(57)【要約】【課題】連続体ロボットの湾曲部を駆動させる際に、当該湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との接触を防止する仕組みを提供する。【解決手段】切替判定部１２２は、運動学演算部１２１で算出された、湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との距離を表す距離情報である座標値ｚe1（θe）に応じて、当該座標値とともに運動学演算部１２１で算出された当該湾曲部を駆動させるためのワイヤの駆動変位量に基づくワイヤの駆動を行うか否かを判定する。【選択図】図１
2018176406.tif 000002
【特許請求の範囲】
【請求項１】
基準面を介して延伸する複数のワイヤと、
前記複数のワイヤが異なる位置に固定され、前記複数のワイヤを案内する第１のワイヤガイドと、
前記基準面と前記第１のワイヤガイドとの間に設けられ、前記複数のワイヤのうちの一部のワイヤが固定された、前記複数のワイヤを案内する第２のワイヤガイドとを含み形成され、
前記複数のワイヤのうちの少なくとも一部の所定ワイヤを駆動することにより湾曲可能な湾曲部を、有する連続体ロボットの制御システムであって、
前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記所定ワイヤの駆動変位量と、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との距離を表す距離情報とを算出する算出手段と、
前記所定ワイヤの駆動を制御する制御手段と、を有し、
前記制御手段は、前記距離情報とともに前記算出手段で算出された前記駆動変位量に基づいて前記所定ワイヤの駆動を行うか否かを、前記距離情報に応じて判定する判定手段を有することを特徴とする連続体ロボットの制御システム。
【請求項２】
前記制御手段は、前記距離情報が閾値以下であった場合、当該距離情報とともに前記算出手段で算出された前記駆動変位量に基づく前記所定ワイヤの駆動を行わないように制御することを特徴とする請求項１に記載の連続体ロボットの制御システム。
【請求項３】
前記制御手段は、前記距離情報が閾値以下であった場合、予め記憶部に記憶されている前記所定ワイヤの駆動変位量に基づく前記所定ワイヤの駆動を行う制御をすることを特徴とする請求項１に記載の連続体ロボットの制御システム。
【請求項４】
前記制御手段は、前記距離情報が前記閾値以下であった場合、予め記憶部に記憶されている前記所定ワイヤの駆動変位量に基づく前記所定ワイヤの駆動を行う制御をすることを特徴とする請求項２に記載の連続体ロボットの制御システム。
【請求項５】
前記記憶部に記憶されている前記所定ワイヤの駆動変位量は、前記距離情報が前記閾値以下でなかった場合に当該距離情報とともに前記算出手段により算出された前記駆動変位量であることを特徴とする請求項３または４に記載の連続体ロボットの制御システム。
【請求項６】
前記湾曲部は、複数の前記第２のワイヤガイドを含んで構成され、
前記算出手段は、前記距離情報として、前記複数の第２のワイヤガイドのうち前記基準面に最も近い第２のワイヤガイドにおける前記近位端と当該基準面との距離を表す距離情報を算出することを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
【請求項７】
前記少なくとも一部の所定ワイヤは複数の所定ワイヤを有し、
前記算出手段は、当該複数の所定ワイヤを構成するワイヤごとに前記駆動変位量を算出することを特徴とする請求項１乃至６のいずれか１項に記載の連続体ロボットの制御システム。
【請求項８】
前記湾曲部の長さは、当該湾曲部の湾曲形状に応じて変化することを特徴とする請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システム。
【請求項９】
前記連続体ロボットの長軸方向に直列に複数の前記湾曲部が構成され、
前記算出手段は、前記複数の湾曲部の湾曲部ごとに、前記所定ワイヤの駆動変位量と前記距離情報とを算出し、
前記制御手段は、前記複数の湾曲部の湾曲部ごとに、前記制御を行うことを特徴とする請求項１乃至８のいずれか１項に記載の連続体ロボットの制御システム。
【請求項１０】
前記第２のワイヤガイドは、前記複数のワイヤのうちの前記所定ワイヤを除く他のワイヤが所定の位置に固定されていることを特徴とする請求項１乃至９のいずれか１項に記載の連続体ロボットの制御システム。
【請求項１１】
前記湾曲部は、前記複数のワイヤのうちの全部のワイヤを前記所定ワイヤとして駆動することにより湾曲可能に構成されていることを特徴とする請求項１乃至９のいずれか１項に記載の連続体ロボットの制御システム。
【請求項１２】
基準面を介して延伸する複数のワイヤと、前記複数のワイヤが異なる位置に固定され、前記複数のワイヤを案内する第１のワイヤガイドと、前記基準面と前記第１のワイヤガイドとの間に設けられ、前記複数のワイヤのうちの一部のワイヤが固定された、前記複数のワイヤを案内する第２のワイヤガイドとを含み形成され、前記複数のワイヤのうちの少なくとも一部の所定ワイヤを駆動することにより湾曲可能な湾曲部を、有する連続体ロボットの制御方法であって、
前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記所定ワイヤの駆動変位量と、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との距離を表す距離情報とを算出する算出ステップと、
前記距離情報とともに前記算出ステップで算出された前記駆動変位量に基づいて前記所定ワイヤの駆動を行うか否かを、前記距離情報に応じて判定する判定ステップと、
を有することを特徴とする連続体ロボットの制御方法。
【請求項１３】
請求項１乃至１１のいずれか１項に記載の連続体ロボットの制御システムにおける各手段としてコンピュータを機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、連続体ロボットの制御システム及びその制御方法、並びに、当該制御システムとしてコンピュータを機能させるためのプログラムに関するものである。
【背景技術】
【０００２】
連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ湾曲部を有し、その湾曲部を変形させることにより形状が制御される。この連続体ロボットは、剛体リンクにより構成されるロボットに対して、主に２つの優位性を持つ。１つ目は、連続体ロボットは、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。２つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。
【０００３】
この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。この連続体ロボットの駆動方法としては、腱駆動によるもの、押引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。非特許文献１では、３本のワイヤと、それをガイドするベースディスク、スペーサーディスク、エンドディスクにより連続体ロボットを構成し、運動学を導出することでワイヤの押し引き駆動量を求め、形状の制御を行っている。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】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を図示している。また、図２では、第１のワイヤガイド１３２１の遠位端１３２１４において、中心軸１３０４が通る位置１３２１５を図示している。また、図２では、位置１３２１５において、中心軸１３０４とｚ軸と平行な線分とのなす角度を湾曲部１３２０の湾曲角度θ1として図示している。また、図２では、中心軸１３０４をｘｙ面に投影したｗ軸を図示し、また、位置１３２１５をｘｙ面に投影した位置１３０５を図示している。また、図２では、ｘ軸とｗ軸とのなす角度を湾曲部１３２０の旋回角度ζ1として図示している。また、図２では、中心軸１３０４に係る湾曲部１３２０の曲率半径ρ1を図示している。また、図２では、第１のワイヤガイド１３２１の遠位端１３２１４から第２のワイヤガイド１３２５の近位端１３２５２までのワイヤ１３０１の長さｌaw1を図示している。
【００２０】
ここで、再び、図１の説明に戻る。
入力装置１１０は、湾曲部１３２０の湾曲形状パラメータである、湾曲部１３２０の湾曲角度θ1の目標値である目標湾曲角度θref1、及び、湾曲部１３２０の旋回角度ζ1の目標値である目標旋回角度ζ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（θ）と表す。
【００３０】
【数１】
2018176406.tif 000003
【００３１】
また、制御装置１２０の具体的な処理例では、以下に示す仮定に基づき、連続体ロボットの運動学を導出する。
［１］湾曲部（湾曲部が複数ある場合には、各湾曲部）において、ワイヤは、曲率一定に変形する。
［２］ワイヤのねじり変形は考慮しない。
［３］ワイヤは、長手方向に変形しない。
［４］ワイヤガイドとワイヤとの間の摩擦は考慮しない。
【００３２】
本実施形態では、まず、湾曲部１３２０の湾曲角度θ1と第１のワイヤガイド１３２１の遠位端１３２１４の座標（ｘt1，ｙt1，ｚt1）との関係を導出する。
【００３３】
湾曲部１３２０の湾曲角度θ1は、図２に示すｗｚ面内の第１のワイヤガイド１３２１の遠位端１３２１４における湾曲角度に相当する。ここで、ｗｚ面内における湾曲部１３２０の遠位端１３２１４の座標をｗt1,ｚt1とすると、座標ｗt1及びｚt1は、上述した湾曲部１３２０の湾曲角度θ1及び湾曲部１３２０の曲率半径ρ1を用いて、それぞれ、以下の（１）式及び（２）式で表すことができる。
【００３４】
【数２】
2018176406.tif 000004
【００３５】
また、上述した湾曲部１３２０の中心軸の長さｌ1dは、以下の（３）式で表すことができる。
【００３６】
【数３】
2018176406.tif 000005
【００３７】
そして、この（３）式を、上述した（１）式及び（２）式に代入すると、それぞれ、以下の（４）式及び（５）式が導かれる。
【００３８】
【数４】
2018176406.tif 000006
【００３９】
ここで、本実施形態においては、３本のワイヤ１３０１〜１３０３のうちの２本のワイヤ１３０２及び１３０３を駆動させることを考えると、旋回角度ζ1の変動により湾曲部１３２０の中心軸の長さｌ1dが変動してしまい、（４）式及び（５）式では遠位端１３２１４の座標が求まらない。そこで、本実施形態では、図３に示すようにｘｙ面を用いてワイヤ１３０１〜１３０３の配置を定義し、湾曲部１３２０の中心軸の長さｌ1dを求める。
【００４０】
図３は、図２に示すワイヤ１３０１〜１３０３の配置をｘｙ面において定義するための模式図である。図３では、各ワイヤ１３０１〜１３０３は、一片の長さがｒsの正三角形の頂点に配置されており、位相角ξn（本実施形態では、位相角ξ1とする）は各ワイヤ１３０１〜１３０３の配置を決定する角度である。本実施形態では、ξ1＝０とする。
【００４１】
ここで、連続体ロボット１３０の湾曲部１３２０をｗｚ面へ投影することを考える。
図４は、図２に示す連続体ロボット１３０の湾曲部１３２０をｗｚ面へ投影した場合の一例を示す模式図である。この図４において、図２に示す構成と同様の構成については同じ符号を付している。この図４に示す、ワイヤ１３０１のｗｚ面における原点０からの距離ｒ1a、ワイヤ１３０２のｗｚ面における原点０からの距離ｒ1b、及び、ワイヤ１３０３のｗｚ面における原点０からの距離ｒ1cは、それぞれ、以下の（６−１）式、（６−２）式及び（６−３）式のように表せる。
【００４２】
【数５】
2018176406.tif 000007
【００４３】
ここで、固定ワイヤであるワイヤ１３０１の長さは不変であるため、ワイヤ１３０１の長さをｌ1aは、図４と（６−１）式により、以下の（７）式のように表せる。
【００４４】
【数６】
2018176406.tif 000008
【００４５】
これより、ｗｚ面における湾曲部１３２０の中心軸の長さｌ1dは、以下の（８）式のように表せる。
【００４６】
【数７】
2018176406.tif 000009
【００４７】
この（８）式を、上述した（４）式及び（５）式に代入すると、それぞれ、以下の（９）式及び（１０）式が導かれる。
【００４８】
【数８】
2018176406.tif 000010
【００４９】
これより、ｘｙｚ空間における第１のワイヤガイド１３２１の遠位端１３２１４の座標（ｘt1，ｙt1，ｚt1）は、それぞれ、以下の（１１）式〜（１３）となる。
【００５０】
【数９】
2018176406.tif 000011
【００５１】
なお、ワイヤ１３０２の長さｌ1b、及び、ワイヤ１３０３の長さｌ1cは、それぞれ、以下の（１４）式及び（１５）となる。
【００５２】
【数１０】
2018176406.tif 000012
【００５３】
本実施形態では、第１のワイヤガイド１３２１の遠位端１３２１４の中心座標に加えて、遠位端１３２１４から長さｌaw1においてワイヤ１３０１に固定されている第２のワイヤガイド１３２５の近位端１３２５２の外周上の座標（ｘe1，ｙe1，ze1）を求める。
【００５４】
この際、上述したワイヤ１３０１の長さｌ1a及び長さｌaw1を用いて長さｌw1と、更には、湾曲部１３２０の湾曲角度θ1を用いて角度θw1とを、以下の（１６）式のように定義する。
【００５５】
【数１１】
2018176406.tif 000013
【００５６】
そして、この（１６）式の関係を用いて、第２のワイヤガイド１３２５の近位端１３２５２における中心座標（ｘe10，ｙe10，ｚe10）は、以下の（１７）式〜（１９）式となる。
【００５７】
【数１２】
2018176406.tif 000014
【００５８】
このことから、第２のワイヤガイド１３２５の近位端１３２５２における外周上の座標は、以下の（２０）式のようになる。なお、（２０）式において、角度θeは、（２１）式に示す範囲を採る。
【００５９】
【数１３】
2018176406.tif 000015
【００６０】
次いで、湾曲部１３２０におけるワイヤ１３０２の駆動変位量ｌp1b及びワイヤ１３０３の駆動変位量ｌp1cと湾曲部１３２０の湾曲角度θ1との関係を導出する。ｗｚ面内において、湾曲部１３２０のみを考えると、ワイヤ１３０２の駆動変位量ｌp1bと湾曲部１３２０の湾曲角度θ1との関係、及び、ワイヤ１３０３の駆動変位量ｌp1cと湾曲部１３２０の湾曲角度θ1との関係は、それぞれ、以下の（２２）式及び（２３）式となる。
【００６１】
【数１４】
2018176406.tif 000016
【００６２】
運動学演算部１２１は、入力装置１１０から目標湾曲角度θref1及び目標旋回角度ζref1が入力されると、これらをそれぞれ、（２２）式及び（２３）式のθ1及びζ1に代入して、駆動変位量ｌ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の最大値＞
次いで、運動学演算部１２１による運動学を用いて、湾曲角度θ1の最大値を算出する場合について説明する。
湾曲角度θ1の最大値を算出する場合、湾曲角度θ1の範囲は、第１のワイヤガイド１３２１の遠位端１３２１４における基台部１３１０の上面１３１４との接触と、第２のワイヤガイド１３２５の近位端１３２５２における基台部１３１０の上面１３１４との接触により定まる。ここでは、前者による湾曲角度θ1の制約を２７０度に設定し、本実施形態では、後者が原因となる湾曲角度θ1の最大値を算出する。この場合、例えば、旋回角度ζ1を０度から１度刻みで３５９度まで変化させ、各旋回角度ζ1において湾曲角度θ1を０度から1度刻みで２７０度まで変化させる。
【００７５】
上述した（２０）式により座標値ｚe1（θe）を算出し、それが閾値（ここでは０とする）以上となる範囲の最大湾曲角度をθ1maxとして、シミュレーションした結果を図６に示す。すなわち、図６は、本発明の第１の実施形態を示し、各旋回角度ζ1と最大湾曲角度θ1maxとの関係をシミュレーションした結果を示す図である。この図６により、旋回角度ζ1が１８０度において、最大湾曲角度θ1maxが最小となることがわかる。これは、旋回方向が原点Ｏからみたワイヤ１３０１の方向と逆方向になる旋回角度ζ1において、近位端１３２５２と基台部１３１０の上面１３１４とが接触する湾曲角度が最も小さくなることを示している。
【００７６】
以上説明したように、第１の実施形態では、切替判定部１２２は、運動学演算部１２１で算出された座標値ｚe1（θe）に応じて、当該座標値とともに運動学演算部１２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行うか否かを制御（駆動制御）するようにしている。
かかる構成によれば、例えば挿抜経路４００に沿って連続体ロボット１３０の湾曲部１３２０を駆動させる際に（より詳細に、例えば湾曲部１３２０を挿抜経路４００に接触しないように駆動させる際に）、当該湾曲部の湾曲形状の基準面となる部材（基台部１３１０）と当該湾曲部内の最も基準面に近い部材（第２のワイヤガイド１３２５）との接触を防止することができる。
【００７７】
（第２の実施形態）
次に、本発明の第２の実施形態について説明する。なお、以下の第２の実施形態における説明では、第１の実施形態と共通する部分については必要に応じてその説明を省略し、第１の実施形態と異なる部分を中心に説明を行う。
【００７８】
＜連続体ロボットの制御システムの構成例＞
図７は、本発明の第２の実施形態に係る連続体ロボットの制御システム２００の構成例を示す模式図である。連続体ロボットの制御システム２００は、図７に示すように、入力装置２１０と、制御装置２２０と、連続体ロボット２３０とを有して構成されている。
【００７９】
≪連続体ロボットの構成例≫
まず、図７に示す連続体ロボット２３０の構成例について説明する。
図８は、図７に示す連続体ロボット２３０の構成例を示す模式図である。この図８において、図２に示す構成と同様の機能を有する構成については同じ符号を付しており、その詳細な説明は省略する。また、図８においては、図２に示す基台部１３１０の上面１３１４のみを図示し、当該上面１３１４よりも下方に位置する基台部１３１０の内部構成は省略している。連続体ロボット２３０は、図８に示すように、基台部１３１０（図８では、基台部１３１０の上面１３１４のみ図示）と、第１湾曲部１３２０と、第２湾曲部１３３０と、第３湾曲部１３４０を有して構成されている。ここで、図８においては、図２に示す湾曲部１３２０に相当する構成を「第１湾曲部１３２０」としている。
【００８０】
図８に示す連続体ロボット２３０は、長軸方向に直列に複数の湾曲部１３２０〜１３４０を有し、複数の湾曲部１３２０〜１３４０の長軸方向に移動可能に構成されている。なお、図８に示す例では、３つの湾曲部を備える連続体ロボットを示しているが、本実施形態においてはこの態様に限定されるものではない。例えば２つの湾曲部を備える連続体ロボットや４つ以上の湾曲部を備える連続体ロボットも、本実施形態に適用可能である。
【００８１】
第１湾曲部１３２０は、当該第１湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４を介して延伸する複数のワイヤ１３０１−１〜１３０３−１と、複数のワイヤ１３０１−１〜１３０３−１が異なる位置に固定され、複数のワイヤ１３０１−１〜１３０３−１を案内する第１のワイヤガイド１３２１と、基台部１３１０の上面１３１４と第１のワイヤガイド１３２１との間に複数のワイヤ１３０１−１〜１３０３−１のうちの一部のワイヤ１３０１−１が所定の位置に固定されて設けられ、複数のワイヤ１３０１−１〜１３０３−１を案内する第２のワイヤガイド１３２２〜１３２５と、を含み形成されている。この第１湾曲部１３２０においては、当該第１湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４を基準として、図２に示すｘ軸に相当する「ｘ1軸」、図２に示すｙ軸に相当する「ｙ1軸」、図２に示すｚ軸に相当する「ｚ1軸」、図２に示すｗ軸に相当する「ｗ1軸」が定義される。また、この第１湾曲部１３２０において、第１のワイヤガイド１３２１の遠位端１３２１４、及び、第２のワイヤガイド１３２５の近位端１３２５２を示している。この場合、第１湾曲部１３２０は、基台部１３１０の上面１３１４（厳密に言えば、上面１３１４自体は含まない）から第１のワイヤガイド１３２１の遠位端１３２１４に至るまでとなっている。その他、第１湾曲部１３２０に関する構成等については、上述した第１の実施形態における湾曲部１３２０と同様である。
【００８２】
第２湾曲部１３３０は、当該第２湾曲部１３３０の基準面に相当する第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面を介して延伸する複数のワイヤ１３０１−２〜１３０３−２と、複数のワイヤ１３０１−２〜１３０３−２が異なる位置に固定され、複数のワイヤ１３０１−２〜１３０３−２を案内する第１のワイヤガイド１３３１と、第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面と第１のワイヤガイド１３３１との間に複数のワイヤ１３０１−２〜１３０３−２のうちの一部のワイヤ１３０１−２が所定の位置に固定されて設けられ、複数のワイヤ１３０１−２〜１３０３−２を案内する第２のワイヤガイド１３３２〜１３３５と、を含み形成されている。この第２湾曲部１３３０においては、当該第２湾曲部１３３０の基準面に相当する第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面を基準として、図２に示すｘ軸に相当する「ｘ2軸」、図２に示すｙ軸に相当する「ｙ2軸」、図２に示すｚ軸に相当する「ｚ2軸」、図２に示すｗ軸に相当する「ｗ2軸」が定義される。また、この第２湾曲部１３３０において、第１のワイヤガイド１３３１の遠位端１３３１４、及び、第２のワイヤガイド１３３５の近位端１３３５１を示している。この場合、第２湾曲部１３３０は、第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面（厳密に言えば、遠位端１３２１４の面自体は含まない）から第１のワイヤガイド１３３１の遠位端１３３１１に至るまでとなっている。また、本実施形態においては、基台部１３１０の内部には、第１の実施形態における構成に加えて、ワイヤ１３０２−２を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１１に相当するアクチュエータ）と、ワイヤ１３０３−２を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１２に相当するアクチュエータ）と、ワイヤ１３０１−２の一端を固定する固定部（図２の固定部１３１３に相当する固定部）も設けられている。すなわち、図８に示す連続体ロボット２３０では、これらのアクチュエータを制御してワイヤ１３０２−２及び１３０３−２を駆動することにより、第２湾曲部１３３０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。
【００８３】
第３湾曲部１３４０は、当該第３湾曲部１３４０の基準面に相当する第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面を介して延伸する複数のワイヤ１３０１−３〜１３０３−３と、複数のワイヤ１３０１−３〜１３０３−３が異なる位置に固定され、複数のワイヤ１３０１−３〜１３０３−３を案内する第１のワイヤガイド１３４１と、第２湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３３１１の面と第１のワイヤガイド１３４１との間に複数のワイヤ１３０１−３〜１３０３−３のうちの一部のワイヤ１３０１−３が所定の位置に固定されて設けられ、複数のワイヤ１３０１−３〜１３０３−３を案内する第２のワイヤガイド１３４２〜１３４５と、を含み形成されている。この第３湾曲部１３４０においては、当該第３湾曲部１３３０の基準面に相当する第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面を基準として、図２に示すｘ軸に相当する「ｘ3軸」、図２に示すｙ軸に相当する「ｙ3軸」、図２に示すｚ軸に相当する「ｚ3軸」、図２に示すｗ軸に相当する「ｗ3軸」が定義される。また、この第２湾曲部１３４０において、第１のワイヤガイド１３４１の遠位端１３４１１、及び、第２のワイヤガイド１３４５の近位端１３４５１を示している。この場合、第３湾曲部１３４０は、第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面（厳密に言えば、遠位端１３３１１の面自体は含まない）から第１のワイヤガイド１３４１の遠位端１３４１１に至るまでとなっている。また、本実施形態においては、基台部１３１０の内部には、更に、ワイヤ１３０２−３を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１１に相当するアクチュエータ）と、ワイヤ１３０３−３を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１２に相当するアクチュエータ）と、ワイヤ１３０１−３の一端を固定する固定部（図２の固定部１３１３に相当する固定部）も設けられている。すなわち、図８に示す連続体ロボット２３０では、これらのアクチュエータを制御してワイヤ１３０２−３及び１３０３−３を駆動することにより、第３湾曲部１３４０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。
【００８４】
また、図８に示す連続体ロボット２３０は、第１のワイヤガイド１３４１よりも上面１３１４から遠い方にカメラ（不図示）が設けられている。これにより、湾曲部１３２０〜１３４０を挿抜経路４００に沿って挿入することにより、挿抜経路４００の内部を観察可能となっている。
【００８５】
なお、本実施形態においては、各湾曲部１３２０〜１３４０によって基準面が異なることから、図８では、各湾曲部の湾曲角度θ1，θ2，θ3と、各湾曲部の旋回角度ζ1，ζ2，ζ3に、後述する相対座標系であることを示す符号（〜）を上に付している。
【００８６】
ここで、再び、図７の説明に戻る。なお、図７に示す例では、連続体ロボット２３０にｅ個の湾曲部が設けられている場合を示している。この際、図８に示す連続体ロボット２３０の制御を行う場合には、ｅ＝３となる。
【００８７】
入力装置２１０は、ｅ個の湾曲部における各湾曲部の湾曲形状パラメータである、当該各湾曲部の湾曲角度θの目標値である目標湾曲角度θ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と置き換えて、第ｍ湾曲部のワイヤが配置されているとき、第ｍ湾曲部の第１湾曲部に対する相対的な位相角を以下のようにして、（２４）式と定義する。
【０１００】
【数１５】
2018176406.tif 000017
【０１０１】
これにより、第ｍ湾曲部の第１のワイヤガイドの遠位端に接続されているワイヤ１３０１の第１湾曲部のおける長さｌma1、この場合のワイヤ１３０２の長さｌmb1及びワイヤ１３０３の長さｌmc1は、それぞれ、以下の（２５）式〜（２７）式となる。
【０１０２】
【数１６】
2018176406.tif 000018
【０１０３】
続いて、第２湾曲部を考えると、区間内のワイヤ１３０１の長さｌ2aは、第２湾曲部のワイヤ１３０１の全長（２ｌ）から第１湾曲部の内部を通過する第２湾曲部のワイヤ１３０１の長さ（ｌ2a1）を差し引けばよいから、以下の（２８）式により求まる。
【０１０４】
【数１７】
2018176406.tif 000019
【０１０５】
ここで、第２湾曲部まで中心軸回りの旋回角度の総和から位相角ξ2を差し引いた角度ν2を、以下の（２９）式により定義する。
【０１０６】
【数１８】
2018176406.tif 000020
【０１０７】
そうすると、第２湾曲部における中心軸の長さｌ2dと、その場合のワイヤ１３０２の長さｌ2b及びワイヤ１３０３の長さｌ2cは、それぞれ、以下の（３０）式〜（３２）式となる。
【０１０８】
【数１９】
2018176406.tif 000021
【０１０９】
このように、第２湾曲部における中心軸の長さｌ2dは、（２８）式に示すワイヤ１３０１の長さｌ2a1を用いて求めることができる。さらに、第２湾曲部以降の湾曲部の中心軸の長さを求めるために、第２湾曲部を通過する第ｍ湾曲部のワイヤの長さを求める。第ｍ湾曲部の第２湾曲部に対する相対的な位相角を以下のようにして、以下の（３３）式と定義する。
【０１１０】
【数２０】
2018176406.tif 000022
【０１１１】
これにより、第ｍ湾曲部の第１のワイヤガイドの遠位端に接続されているワイヤ１３０１の第２湾曲部のおける長さｌma2、この場合のワイヤ１３０２の長さｌmb2及びワイヤ１３０３の長さｌmc2は、それぞれ、以下の（３４）式〜（３６）式となる。
【０１１２】
【数２１】
2018176406.tif 000023
【０１１３】
このように、各湾曲部の中心軸の長さは、基台部１３１０に近い湾曲部から順に求めることができる。一般化し、第ｎ湾曲部を考えると、区間内のワイヤ１３０１の長さｌnaは、以下の（３７）式で求まる。
【０１１４】
【数２２】
2018176406.tif 000024
【０１１５】
ここで、（２９）式に示す角度を一般化した角度νnを、以下の（３８）式により定義する。
【０１１６】
【数２３】
2018176406.tif 000025
【０１１７】
そうすると、第ｎ湾曲部における中心軸の長さｌndと、その場合のワイヤ１３０２の長さｌnb及びワイヤ１３０３の長さｌncは、それぞれ、以下の（３９）式〜（４１）式となる。
【０１１８】
【数２４】
2018176406.tif 000026
【０１１９】
さらに、（３３）式を一般化して、以下の（４２）式と定義する。
【０１２０】
【数２５】
2018176406.tif 000027
【０１２１】
これにより、第ｎ湾曲部を通過する第ｍ湾曲部のワイヤ１３０１の長さｌman、この場合のワイヤ１３０２の長さｌmbn及びワイヤ１３０３の長さｌmcnは、それぞれ、以下の（４３）式〜（４５）式となる。
【０１２２】
【数２６】
2018176406.tif 000028
【０１２３】
以上の手順で求められる第ｎ湾曲部の中心軸の長さｌndを用いると、相対座標系（ｘn，ｙn，ｚn）における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標は、以下の（４６）式〜（４８）式となる。
【０１２４】
【数２７】
2018176406.tif 000029
【０１２５】
これより、絶対座標系における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標（ｘtn，ｙtn，ｚtn）は、回転変換行列を用いて、以下の（４９）式となる。
【０１２６】
【数２８】
2018176406.tif 000030
【０１２７】
第ｎ湾曲部の第１のワイヤガイドの遠位端から長さｌawnにある第２のワイヤガイドの近位端の外周の座標については、以下に示す相対座標系における座標を求める。また、当該第２のワイヤガイドの中心座標を以下に示す座標として記載する。
【０１２８】
【数２９】
2018176406.tif 000031
【０１２９】
そして、以下の（５０）式を定義する。
【０１３０】
【数３０】
2018176406.tif 000032
【０１３１】
この場合、上述した第２のワイヤガイドの中心座標は、以下の（５１）式〜（５３）式となる。
【０１３２】
【数３１】
2018176406.tif 000033
【０１３３】
このことから、上述した第ｎ湾曲部の第１のワイヤガイドの遠位端から長さｌawnにある第２のワイヤガイドの近位端の外周の相対座標系における座標は、以下の（５４）式となる。
【０１３４】
【数３２】
2018176406.tif 000034
【０１３５】
次いで、第ｎ湾曲部におけるワイヤ１３０２の駆動変位量ｌpnb及びワイヤ１３０３の駆動変位量ｌpncと、第ｎ湾曲部の湾曲角度θn及び旋回角度ζnとの関係を導出する。ただし、ｎは２以上とする。相対座標系（ｘn，ｙn，ｚn）における各ワイヤの駆動変位量と第1湾曲部の湾曲角度との関係は、以下の（５５）式〜（５６）式のようになる。
【０１３６】
【数３３】
2018176406.tif 000035
【０１３７】
第ｎ湾曲部におけるワイヤ１３０２及び１３０３の駆動変位量ｌpnb，ｌpncは、第１湾曲部から第ｎ湾曲部までの相対座標系におけるワイヤの駆動変位量の総和となり、以下の（５７）式となる。
【０１３８】
【数３４】
2018176406.tif 000036
【０１３９】
ここでは、図７に示す入力装置２１０から入力する湾曲形状パラメータを、以下の（５８）式で表す。
【０１４０】
【数３５】
2018176406.tif 000037
【０１４１】
この場合、運動学演算部２２１は、入力装置１１０から、上述の（５８）式に示す湾曲形状パラメータが入力されると、各湾曲部ごとに、以下に示す、基準面と当該基準面に最も近い第２のワイヤガイドの近位端との距離を表す距離情報である座標値を算出する。
【０１４２】
【数３６】
2018176406.tif 000038
【０１４３】
そして、切替判定部２２２は、各湾曲部ごとに、上述した座標値が閾値以下であるか否かを判断し、当該判断の結果に基づいて、連続体ロボット２３０の駆動制御を行う。この具体的な連続体ロボット２３０の駆動制御は、図７を用いて上述した通りである。
【０１４４】
＜湾曲角度の最大値＞
次いで、運動学演算部２２１による運動学を用いて、図８に示す第３湾曲部１３４０の湾曲角度θ3の最大値を算出する場合について説明する。
ここでは、直立姿勢時の各湾曲部１３２０〜１３４０の中心軸１３０４の長さｌを０．０１０ｍとする。また、直立姿勢時の各湾曲部１３２０〜１３４０における長さｌaw1〜ｌaw3は、いずれも０．００９２ｍとする。また、第１実施形態と同様に、第１のワイヤガイドの遠位端における湾曲角度の制約は２７０度と設定し、本実施形態では、基準面に最も近い第２のワイヤガイドの近位端の接触が原因となる湾曲角度θ3の最大値を算出する。第３湾曲部１３４０の湾曲角度θ3の最大値は、それよりも基台部１３１０に近い第１湾曲部１３２０及び第２湾曲部１３３０の角度により変動する。そこで、ここでは、第１湾曲部１３２０及び第２湾曲部１３３０の湾曲形状パラメータを、以下のａ）〜ｄ）に示す４通りでシミュレーションを行う。
【０１４５】
【数３７】
2018176406.tif 000039
【０１４６】
この際、第１の実施形態と同様に、旋回角度ζ3を０度から１度刻みで３５９度まで変化させ、各旋回角度ζ3において湾曲角度θ3を０度から１度刻みで２７０度まで変化させる。
【０１４７】
上述した（２０）式を用いて座標値ｚe3（θe）を算出し、それが閾値（ここでは０とする）以上となる範囲の最大湾曲角度をθ3maxとする。第３湾曲部１３４０を湾曲させない状態での、上述したａ）〜ｄ）に示す湾曲形状パラメータに基づく連続体ロボット２３０の湾曲形状を、図９（ａ）〜図９（ｄ）にそれぞれ示す。また、第３湾曲部１３４０を湾曲させない状態での、上述したａ）〜ｄ）に示す湾曲形状パラメータに対応するシミュレーション応答を、図１０にそれぞれ実線、破線、一点鎖線、点線で示す。
【０１４８】
図１０において実線及び破線で示す、上述したａ）及びｂ）に示す湾曲形状パラメータでは、第３湾曲部１３４０の最大湾曲角度θ3max及びそれが最小となる旋回角度ζ3は、第２湾曲部１３３０が湾曲することにより変動することがわかる。さらに、図１０において一点鎖線で示す、上述したｃ）に示す湾曲形状パラメータでは、第２湾曲部１３３０に加えて第１湾曲部１３２０を駆動しているが、図９（ｃ）からわかるように、第３湾曲部１３４０の中心軸の長さが長くなる姿勢であるため、第３湾曲部１３４０の最大湾曲角度θ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とすると、以下の（５９）式〜（６１）式となる。
【０１６４】
【数３８】
2018176406.tif 000040
【０１６５】
これにより、絶対座標系における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標（ｘtn，ｙtn，ｚtn）は、回転変換行列を用いて、以下の（６２）式となる。
【０１６６】
【数３９】
2018176406.tif 000041
【０１６７】
第ｎ湾曲部の第１のワイヤガイドの遠位端の中心から長さｌenにある第２のワイヤガイドの近位端の外周の変位については、以下に示す相対座標系における座標を求める。また、当該第２のワイヤガイドの中心座標を以下に示す座標として記載する。
【０１６８】
【数４０】
2018176406.tif 000042
【０１６９】
そして、以下の（６３）式を定義する。
【０１７０】
【数４１】
2018176406.tif 000043
【０１７１】
この場合、上述した第２のワイヤガイドの中心座標は、以下の（６４）式〜（６６）式となる。
【０１７２】
【数４２】
2018176406.tif 000044
【０１７３】
このことから、上述した第ｎ湾曲部の第１のワイヤガイドの遠位端の中心から長さｌenにある第２のワイヤガイドの近位端の外周の相対座標系における座標は、以下の（６７）式となる。
【０１７４】
【数４３】
2018176406.tif 000045
【０１７５】
次いで、第ｎ湾曲部のワイヤ１３０１〜１３０３の駆動変位量ｌpna，ｌpnb，ｌpncと、第ｎ湾曲部の湾曲角度θn及び旋回角度ζnとの関係を導出する。ただし、ｎは２以上とする。相対座標系（ｘn，ｙn，ｚn）における各ワイヤの駆動変位量と第1湾曲部の湾曲角度との関係は、以下の（６８）式〜（７０）式のようになる。
【０１７６】
【数４４】
2018176406.tif 000046
【０１７７】
第ｎ湾曲部におけるワイヤ１３０１〜１３０３の駆動変位量ｌpna，ｌpnb，ｌpncは、第１湾曲部から第ｎ−１湾曲部までの相対座標系における第ｎ湾曲部を駆動するためのワイヤの変位の総和となり、以下の（７１）式となる。
【０１７８】
【数４５】
2018176406.tif 000047
【０１７９】
なお、図１１に示す第３の実施形態に係る連続体ロボットの制御システム３００では、図８に示す第２の実施形態の連続体ロボット２３０のワイヤ１３０１も駆動させる例を示したが、本発明においてはこの形態に限定されるものではない。例えば、第３の実施形態に係る連続体ロボットの制御システム３００として、図２に示す第１の実施形態の連続体ロボット１３０のワイヤ１３０１も駆動させる形態も、本発明に適用可能である。
【０１８０】
以上説明したように、第３の実施形態では、切替判定部３２２は、運動学演算部３２１で算出された座標値ｚe1（θe），…，ｚee（θe）に応じて、各湾曲部ごとに、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のそれぞれに基づくワイヤ１３０１〜１３０３の駆動を行うか否かを制御（駆動制御）するようにしている。
かかる構成によれば、例えば挿抜経路４００に沿って連続体ロボット３３０の各湾曲部を駆動させる際に（より詳細に、例えば各湾曲部を挿抜経路４００に接触しないように駆動させる際に）、当該湾曲部の湾曲形状の基準面となる部材（基台部１３１０或いは第１のワイヤガイド）と当該湾曲部内の最も基準面に近い部材（第２のワイヤガイド）との接触を防止することができる。
【０１８１】
（第４の実施形態）
次に、本発明の第４の実施形態について説明する。
【０１８２】
上述の第２及び第３の実施形態においては、複数の湾曲部を有する連続体ロボットに対して湾曲部ごとに相対座標系を導入し、運動学の導出と制御系の設計を行う場合を示した。本実施形態では、絶対座標系による第２のワイヤガイドの接触回避を伴う制御を対象とする。
【０１８３】
まず、相対角度から絶対座標系における第ｎ湾曲部の湾曲角度θn及び旋回角度ζn を求める手順を示す。
【０１８４】
第ｎ湾曲部の相対座標系において、各湾曲形状の延長線上に遠位端から長さ１の位置（ｘttn，ｙttn，ｚttn）を取ると、その座標は、以下の（７２）式となる。ただし、以下の座標では、相対座標系であることを示す符号（〜）を上に付している。
【０１８５】
【数４６】
2018176406.tif 000048
【０１８６】
そして、絶対座標系における位置（ｘttn，ｙttn，ｚttn）は、回転変換行列を用いると、以下の（７３）式となる。
【０１８７】
【数４７】
2018176406.tif 000049
【０１８８】
これを用いて、以下の（７４）式に示す単位ベクトルｅznを定義する。
【０１８９】
【数４８】
2018176406.tif 000050
【０１９０】
単位ベクトルｅznは、第ｎ湾曲部の遠位端の絶対座標系における方向を表し、ｚ軸からの角度が湾曲角度θnとなり、単位ベクトルｅznのｘｙ平面への射影のｘ軸からの角度が旋回角度ζnとなる。絶対角度から相対角度を求めるには、単位ベクトルｅznの方向に第ｎ＋１湾曲部に対する相対座標系のｚn+1軸をとり、第ｎ湾曲部の相対座標系のｗnｚn平面上においてｚn+1軸と直交する方向にｘn+1軸をとる。右手系より第ｎ＋１湾曲部の相対座標系が定義されるので、座標軸と単位ベクトルｅzn+1との関係から相対角度が求められる。第１湾曲部は相対角度と絶対角度が等しいので、これを近位端から繰り返すことで、全ての湾曲部の相対角度を求めることができる。
【０１９１】
絶対座標系を用いると、第ｎ湾曲部のワイヤ１３０２の駆動変位量ｌpnb及びワイヤ１３０３の駆動変位量ｌpncは、それぞれ、以下の（７５）式及び（７６）式となる。
【０１９２】
【数４９】
2018176406.tif 000051
【０１９３】
これは、第２の実施形態のような回転変換を必要としないため、演算量が少ない。また、絶対座標系で設定される経路に最遠位湾曲部の角度を追従させる制御では、絶対座標系による制御が簡便となる。しかし、第２のワイヤガイドの接触を回避するためには、上記の手順により絶対角度から相対角度を求め、第２の実施形態に示した接触判断を行う必要があり、演算量が増えてしまう。そこで、絶対座標系において簡易的に第２のワイヤガイドの接触を回避する手法を以下に示す。
【０１９４】
隣接する２つの湾曲部である第ｎ−１湾曲部と第ｎ湾曲部に対し、接触を回避するために第ｎ湾曲部の湾曲角度θnを、その相対座標系における湾曲角度θ~lim に制約することを考える。例えば旋回角度ζn-1と旋回角度ζnが等しい場合、湾曲角度の差分（θn−θn-1）に以下の（７７）式に示す制約を設ければよい。
【０１９５】
【数５０】
2018176406.tif 000052
【０１９６】
一方、例えば旋回角度ζn-1と旋回角度ζnとが９０度の差をもつ場合は、以下の（７８）式に示す制約を設ければよい。
【０１９７】
【数５１】
2018176406.tif 000053
【０１９８】
そこで、旋回角度の差分（ζn−ζn-1）を引数とする関数αを導入して以下の（７９）式で表し、湾曲角度θnを以下の（８０）式に示すように制約すればよい。
【０１９９】
【数５２】
2018176406.tif 000054
【０２００】
このようにすることで、絶対座標系においても隣接する湾曲部のワイヤガイド同士が接触することを回避することが可能となる。
【０２０１】
（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
【０２０２】
なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
【符号の説明】
【０２０３】
１００：連続体ロボットの制御システム、１１０：入力装置、１２０：制御装置、１２１：運動学演算部、１２２：切替判定部、１２３：第１の切替部、１２４：記憶部、１２５：第２の切替部、１３０：連続体ロボット
【図１】
2018176406.tif 000055
【図２】
2018176406.tif 000056
【図３】
2018176406.tif 000057
【図４】
2018176406.tif 000058
【図５】
2018176406.tif 000059
【図６】
2018176406.tif 000060
【図７】
2018176406.tif 000061
【図８】
2018176406.tif 000062
【図９】
2018176406.tif 000063
【図１０】
2018176406.tif 000064
【図１１】
2018176406.tif 000065