(57)【要約】【課題】連続体ロボットの操作性の向上を図ることができる仕組みを提供する。【解決手段】入力された遠位湾曲区間の第１の目標湾曲角度に応じて設定される、連続体ロボットの前進移動に対応した追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が第１の目標湾曲角度に至る。遠位湾曲区間を第１の目標湾曲角度に湾曲した後に、第１の湾曲角度のプロファイルに従って追従湾曲区間が湾曲および前進移動し、前進移動の移動量が追従湾曲区間の長さに相当する第１の移動量に至る前に、遠位湾曲区間の目標湾曲角度が、第１の目標湾曲角度から第２の目標湾曲角度へ変更された場合、第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルが設定され、連続体ロボットのさらなる前進駆動により第２の湾曲角度のプロファイルに従って追従湾曲区間の湾曲角度が第２の目標湾曲角度に至る。【選択図】図７
2022002693.tif 000002
【特許請求の範囲】
【請求項１】
基台部と、
遠位用の線状部材が駆動することによって湾曲する遠位湾曲区間と、
前記遠位湾曲区間と前記基台部との間に設けられ、追従用の線状部材が駆動することによって湾曲する追従湾曲区間と、
前記遠位用の線状部材および前記追従用の線状部材を独立に駆動する駆動手段と、
前記基台部、前記遠位湾曲区間、ならびに前記追従湾曲区間を一体にして前進移動させる移動手段と、
制御部と、
を備えた連続体ロボットであって、
前記制御部は、
入力された前記遠位湾曲区間の第１の目標湾曲角度に応じて設定される、前記連続体ロボットの前進移動に対応した前記追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、前記追従湾曲区間の湾曲角度が前記第１の目標湾曲角度に至るよう構成されており、
前記遠位湾曲区間を前記第１の目標湾曲角度に湾曲した後に、前記第１の湾曲角度のプロファイルに従って前記追従湾曲区間が湾曲および前進移動し、前進移動の変位量が前記追従湾曲区間の長さに相当する第１の変位量に至る前に、前記遠位湾曲区間の目標湾曲角度が、前記第１の目標湾曲角度から第２の目標湾曲角度へ変更された場合、前記第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルが設定され、前記連続体ロボットのさらなる前進移動により前記第２の湾曲角度のプロファイルに従って前記追従湾曲区間の湾曲角度が前記第２の目標湾曲角度に至るよう構成されている
ことを特徴とする連続体ロボット。
【請求項２】
前記制御部は、
前記遠位湾曲区間の目標湾曲角度および前記追従湾曲区間の目標湾曲角度と前記基台部の変位との関係を示す参照テーブルを記憶する記憶手段と、
前記遠位湾曲区間の目標湾曲角度の変更に応じて、前記記憶手段に記憶されている前記参照テーブルを書き換える書換手段と、
を含むことを特徴とする請求項１に記載の連続体ロボット。
【請求項３】
前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第１の目標湾曲角度から前記第２の目標湾曲角度に変更された場合に、前記変更の際の前記基台部の変位である第１変位から、前記第１変位から前記追従湾曲区間の長さだけ進んだ変位である第２変位までを底辺の長さとし、前記第２の目標湾曲角度と前記第１の目標湾曲角度との差を高さとする直角三角形を生成し、
前記直角三角形の斜辺上の点に相当する前記湾曲角度から前記底辺に相当する前記湾曲角度までの前記湾曲角度の変化に係る値を、前記記憶手段に記憶されている前記参照テーブルの前記追従湾曲区間の湾曲角度に係る値に加算し、当該加算後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項２に記載の連続体ロボット。
【請求項４】
前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第１の目標湾曲角度から前記第２の目標湾曲角度に変更された場合に、前記第２の目標湾曲角度となった際の前記基台部の変位から、前記追従湾曲区間の長さの和となる変位である第２変位までのうち、前記第１の目標湾曲角度となった際の前記基台部の変位から、前記追従湾曲区間の長さの和となる変位である第１変位までを除いた変位範囲について、前記参照テーブルの前記追従湾曲区間の目標湾曲角度における前記第１変位と前記第２変位との間の変位を、前記第１変位における前記第１の目標湾曲角度に係る値と前記第２変位における前記第２の目標湾曲角度に係る値とを結んだ直線上の前記目標湾曲角度に係る値で補間し、当該補間後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項２に記載の連続体ロボット。
【請求項５】
前記書換手段は、
前記記憶手段に記憶されている前記参照テーブルにおいて、前記遠位湾曲区間の目標湾曲角度が前記第１の目標湾曲角度から前記第２の目標湾曲角度に変更された場合に、前記第２の目標湾曲角度となった際の前記基台部の変位である第１変位から、前記追従湾曲区間の長さの和となる変位である第２変位までの変位範囲について、前記参照テーブルの前記追従湾曲区間の目標湾曲角度における前記第１変位と前記第２変位との間の変位を、前記第１変位における前記追従湾曲区間の目標湾曲角度に係る値と前記第２変位における前記第２の目標湾曲角度に係る値とを結んだ直線上の前記目標湾曲角度に係る値で補間し、当該補間後の追従湾曲区間の目標湾曲角度を前記変更後の追従湾曲区間の目標湾曲角度として、前記参照テーブルを書き換えることを特徴とする請求項２に記載の連続体ロボット。
【請求項６】
前記記憶手段は、異なる複数の前記参照テーブルを記憶しており、
前記書換手段は、前記複数の参照テーブルの中から１つの参照テーブルを選択し、当該選択した参照テーブルを書き換えることを特徴とする請求項２乃至５のいずれか１項に記載の連続体ロボット。
【請求項７】
線状部材が駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記線状部材を駆動する駆動手段と、を備える連続体ロボットの制御方法であって、
複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある遠位湾曲区間の目標湾曲角度と、前記複数の湾曲区間のうち、前記遠位湾曲区間と前記基台部との間に位置する追従湾曲区間の目標湾曲角度と、前記連続体ロボットの進行方向における前記基台部の変位と、前記追従湾曲区間の長さと、に基づいて、前記遠位湾曲区間の目標湾曲角度の変更に伴う変更後の前記追従湾曲区間の目標湾曲角度を算出する算出ステップと、
前記算出ステップで算出された前記変更後の追従湾曲区間の目標湾曲角度に基づいて、前記駆動手段で前記追従湾曲区間の前記線状部材を駆動させる際の駆動変位を演算する演算ステップと、
を有することを特徴とする連続体ロボットの制御方法。
【請求項８】
コンピュータを、請求項１乃至６のいずれか１項に記載の連続体ロボットにおける各手段として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ワイヤ（線状部材）が駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部を備える連続体ロボット及びその制御方法、並びに、当該連続体ロボットとしてコンピュータを機能させるためのプログラムに関するものである。
【背景技術】
【０００２】
連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間が設けられた湾曲可能部を備えて構成されており、その湾曲区間を変形させることによって形状が制御される。この連続体ロボットは、剛体リンクによって構成される剛体リンクロボットに対して、主に２つの優位性を持つ。１つ目は、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、連続体ロボットは、曲線に沿って移動可能である。２つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。
【０００３】
特許文献１には、内視鏡として用いられる連続体ロボットが空間に侵入するための制御方法が記載されている。具体的に、特許文献１では、隣接する湾曲区間の全ての組において、連続体ロボットの基台部の前進に伴って前方湾曲区間の湾曲形状を後続湾曲区間の湾曲形状にするように制御することで、湾曲形状を連続的に伝播させている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１２／０２７１１０９号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１では、連続体ロボットの基台部が湾曲区間の長さを前進するごとに、前方の湾曲区間の湾曲形状を後続の湾曲区間に伝播させる制御方法を開示しているが、湾曲区間の長さに満たない基台部の変位の際に前方の湾曲区間の湾曲形状が変更された場合の後続の湾曲区間への湾曲形状の伝播については、何ら想定されていない。このため、特許文献１に記載の技術では、操縦者の意図と連続体ロボットの形状との乖離が大きくなったり、目標経路への連続体ロボットの侵入が困難となったりして、連続体ロボットの操作性の向上が図れないという問題があった。
【０００６】
本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの操作性の向上を図ることができる仕組みを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の連続体ロボットは、基台部と、遠位用の線状部材が駆動することによって湾曲する遠位湾曲区間と、前記遠位湾曲区間と前記基台部との間に設けられ、追従用の線状部材が駆動することによって湾曲する追従湾曲区間と、前記遠位用の線状部材および前記追従用の線状部材を独立に駆動する駆動手段と、前記基台部、前記遠位湾曲区間、ならびに前記追従湾曲区間を一体にして前進移動させる移動手段と、制御部と、を備えた連続体ロボットであって、前記制御部は、入力された前記遠位湾曲区間の第１の目標湾曲角度に応じて設定される、前記連続体ロボットの前進移動に対応した前記追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、前記追従湾曲区間の湾曲角度が前記第１の目標湾曲角度に至るよう構成されており、前記遠位湾曲区間を前記第１の目標湾曲角度に湾曲した後に、前記第１の湾曲角度のプロファイルに従って前記追従湾曲区間が湾曲および前進移動し、前進移動の変位量が前記追従湾曲区間の長さに相当する第１の変位量に至る前に、前記遠位湾曲区間の目標湾曲角度が、前記第１の目標湾曲角度から第２の目標湾曲角度へ変更された場合、前記第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルが設定され、前記連続体ロボットのさらなる前進移動により前記第２の湾曲角度のプロファイルに従って前記追従湾曲区間の湾曲角度が前記第２の目標湾曲角度に至るよう構成されている。
また、本発明は、上述した連続体ロボットの制御方法、及び、上述した連続体ロボットとしてコンピュータを機能させるためのプログラムを含む。
【発明の効果】
【０００８】
本発明によれば、連続体ロボットの操作性の向上を図ることができる。
【図面の簡単な説明】
【０００９】
【図１Ａ】本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。
【図１Ｂ】図１Ａに示す連続体ロボットの概略構成のうち、１つの湾曲区間に関する詳細な概略構成の一例を示す図である。
【図２】図１Ｂに示す３つのワイヤ（ａワイヤ〜ｃワイヤ）のｘｙ平面における配置例を示す図である。
【図３】本発明の第１の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。
【図４】本発明の第１の実施形態を示し、図１Ａに示す連続体ロボットの運動学モデルの一例を示す図である。
【図５】本発明の第１の実施形態を示し、図１Ａに示す連続体ロボットの運動学モデルの一例を示す図である。
【図６】本発明の第１の実施形態に係る連続体ロボットの先頭追従制御の一例を示す図である。
【図７】本発明の第１の実施形態を示し、図３の後続湾曲角度算出部による最遠位湾曲区間の目標湾曲角度の変更に伴う変更後の後続湾曲区間の目標湾曲角度の算出処理の一例を示す図である。
【図８】本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法における処理手順の一例を示すフローチャートである。
【図９】本発明の第１の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図１０】本発明の第１の実施形態を示し、図９に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【図１１】本発明の第１の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図１２】本発明の第１の実施形態を示し、図１１に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【図１３】本発明の第２の実施形態を示し、図３の後続湾曲角度算出部による最遠位湾曲区間の目標湾曲角度の変更に伴う変更後の後続湾曲区間の目標湾曲角度の算出処理の一例を示す図である。
【図１４】本発明の第２の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図１５】本発明の第２の実施形態を示し、図１４に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【図１６】本発明の第２の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図１７】本発明の第２の実施形態を示し、図１６に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【図１８】本発明の第３の実施形態を示し、図３の後続湾曲角度算出部による最遠位湾曲区間の目標湾曲角度の変更に伴う変更後の後続湾曲区間の目標湾曲角度の算出処理の一例を示す図である。
【図１９】本発明の第３の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図２０】本発明の第３の実施形態を示し、図１９に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【図２１】本発明の第３の実施形態を示し、最遠位湾曲区間の目標湾曲角度における１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。
【図２２】本発明の第３の実施形態を示し、図２１に示すシミュレーション結果を反映した連続体ロボットの湾曲可能部における動作状態の一例を示す図である。
【発明を実施するための形態】
【００１０】
以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。具体的な連続体ロボットは、基台部と、遠位用のワイヤが駆動することによって湾曲する遠位湾曲区間と、遠位湾曲区間と基台部との間に設けられ、追従用のワイヤが駆動することによって湾曲する追従湾曲区間と、を備えている。さらに、連続体ロボットは、遠位用のワイヤおよび追従用のワイヤを独立に駆動する駆動手段と、基台部、遠位湾曲区間、ならびに追従湾曲区間を一体にして前進移動させる移動手段と、制御部を備えている。この制御部は、入力された遠位湾曲区間の第１の目標湾曲角度に応じて設定される、連続体ロボットの前進移動に対応した追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従い、追従湾曲区間の湾曲角度が第１の目標湾曲角度に至るよう構成される。加えて、この制御部は、遠位湾曲区間を第１の目標湾曲角度に湾曲した後に、第１の湾曲角度のプロファイルに従って追従湾曲区間が湾曲および前進移動し、前進移動の移動量が追従湾曲区間の長さに相当する第１の移動量に至る前に、遠位湾曲区間の目標湾曲角度が、第１の目標湾曲角度から第２の目標湾曲角度へ変更された場合、以下のように制御される。すなわち、第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルが設定され、連続体ロボットのさらなる前進移動により、第２の湾曲角度のプロファイルに従って追従湾曲区間の湾曲角度が第２の目標湾曲角度に至るよう構成されている。
【００１１】
（第１の実施形態）
まず、本発明の第１の実施形態について説明する。
【００１２】
図１Ａは、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。図１Ａには、連続体ロボット１００の一構成として、基台部１４０及び湾曲可能部１７０が図示されている。
【００１３】
湾曲可能部１７０は、ワイヤ（線状部材）が駆動することによって湾曲する複数の湾曲区間１７１〜１７３が設けられた構成部（湾曲区間が複数設けられた構成部）である。具体的に、図１Ａには、（ｎ−２）に係る湾曲区間１７１、（ｎ−１）に係る湾曲区間１７２、ｎに係る湾曲区間１７３のｎ個の湾曲区間が図示されている。また、図１Ａでは、湾曲区間１７３の湾曲角度をθnで示し、湾曲区間１７２の湾曲角度をθn-1で示し、湾曲区間１７１の湾曲角度をθn-2で示している。基台部１４０は、湾曲可能部１７０を支える構成部であり、その内部には、湾曲区間１７１〜１７３のそれぞれのワイヤを独立に駆動させるための駆動部（駆動手段）であるアクチュエータ（図１Ａでは不図示）が設けられている。また、図１Ａでは、基台部１４０の上面１４１の所定位置（例えば中心の位置）に原点Ｏをとり、連続体ロボット１００の進行方向（例えば前進する方向）をｚ方向とし、このｚ方向と直交する方向であって相互に直交する方向をｘ方向及びｙ方向とした、ｘｙｚ座標系を図示している。このように、連続体ロボット１００は、湾曲可能部１７０を構成する複数の湾曲区間１７１〜１７３による湾曲動作に加えて、湾曲区間１７１〜１７３を含む湾曲可能部１７０と基台部１４０とが一体に、位置制御されたスライダ（移動手段）によりｚ方向に移動（前進移動）が可能となっている。この際、図１Ａでは、連続体ロボット１００のｚ方向の移動量（前進量、変位量）を示す指標として基台部１４０の変位ｚbを図示している。以下では、遠位湾曲区間として連続体ロボット１００の先頭である最遠位湾曲区間を例に挙げ、また追従湾曲区間として最遠位湾曲区間に連なる後続湾曲区間を例に挙げて、説明することがある。
【００１４】
図１Ａにおいて、湾曲区間１７３は、湾曲可能部１７０を構成する複数の湾曲区間１７１〜１７３のうち、基台部１４０から最も遠い位置にある最遠位湾曲区間である。湾曲区間１７２（湾曲区間１７１も同様）は、最遠位湾曲区間である湾曲区間１７３と基台部１４０との間に位置し、連続体ロボット１００が前進する際に最遠位湾曲区間である湾曲区間１７３の後に続く後続湾曲区間である。なお、連続体ロボット１００が前進する際に（図１Ａのｚ方向の正の向き）、最遠位湾曲区間である湾曲区間１７３は、先頭の湾曲区間となる。
【００１５】
図１Ｂは、図１Ａに示す連続体ロボット１００の概略構成のうち、１つの湾曲区間１７１に関する詳細な概略構成の一例を示す図である。即ち、図１Ｂは、基台部１４０から最も近い位置にある近位端の湾曲区間１７１に関する詳細な概略構成を示している。この図１Ｂにおいて、図１Ａに示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。なお、図１Ｂでは、湾曲区間１７１の湾曲角度をθ1で示し、また、湾曲区間１７１の旋回角度をζ1で示し、湾曲区間１７１の曲率半径（図１Ｂでは、点Ｏと点ｗ１とを結ぶ線分に相当する）をρ1で示している。
【００１６】
連続体ロボット１００は、湾曲区間１７１の遠位端１６０の接続部１２１〜１２３にワイヤ（線状部材）１１１〜１１３が接続され、このワイヤ１１１〜１１３が基台部１４０の内部に設置されているそれぞれのアクチュエータ１３１〜１３３で押し引きされることによって、姿勢（湾曲形状）が制御される。ここで、アクチュエータ１３１は、ワイヤ１１１を駆動させるための駆動部であり、また、アクチュエータ１３２は、ワイヤ１１２を駆動させるための駆動部であり、さらに、アクチュエータ１３３は、ワイヤ１１３を駆動させるための駆動部である。
【００１７】
また、連続体ロボット１００は、湾曲区間１７１において、ワイヤ１１１〜１１３を案内するための部材であるワイヤガイド１６１〜１６４を有する。ワイヤガイド１６１〜１６４は、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１６１〜１６４は、ワイヤ１１１に、固定部１５０〜１５３において固定される。また、図１Ｂにおいて、連続体ロボット１００の中心軸を破線で示している。
【００１８】
また、本実施形態では、ワイヤ１１１〜１１３を、ｘｙ平面内において反時計回りに、ａワイヤ、ｂワイヤ及びｃワイヤと呼ぶ。具体的に、図１Ｂに示す例では、ワイヤ１１１がａワイヤに相当し、湾曲区間１７１のアクチュエータ１３１の押し引きによるワイヤ１１１の駆動変位をｌp1aとして図示している。また、図１Ｂに示す例では、ワイヤ１１２がｂワイヤに相当し、湾曲区間１７１のアクチュエータ１３２の押し引きによるワイヤ１１２の駆動変位をｌp1bとして図示している。さらに、図１Ｂに示す例では、ワイヤ１１３がｃワイヤに相当し、湾曲区間１７１のアクチュエータ１３３の押し引きによるワイヤ１１３の駆動変位をｌp1cとして図示している。
【００１９】
なお、図１Ｂに示す例では、湾曲区間１７１に関する詳細な概略構成のみを図示してその説明を行った。この点、図１Ａに示す湾曲区間１７２及び湾曲区間１７３についても、図１Ｂに示す湾曲区間１７１に関する詳細な概略構成と同様に、それぞれが、ワイヤ（線状部材）１１１〜１１３に相当するワイヤや、アクチュエータ１３１〜１３３に相当するアクチュエータ、遠位端１６０に相当する遠位端、及び、ワイヤガイド１６１〜１６４に相当するワイヤガイドを有して構成されている。ここで、一般化して、第ｎの湾曲区間を駆動するワイヤの駆動変位をｌpna，ｌpnb，ｌpncとする。
【００２０】
図２は、図１Ｂに示す３つのワイヤ１１１〜１１３（ａワイヤ〜ｃワイヤ）のｘｙ平面における配置例を示す図である。図２に示すように、図１Ｂに示す３つのワイヤ１１１〜１１３（ａワイヤ〜ｃワイヤ）は、一片の長さをｒsとする正三角形の頂点に配置されており、また、図２に示す位相角ξnは、第ｎの湾曲区間を駆動するワイヤの配置を決定する角度である。
【００２１】
図３は、本発明の第１の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。連続体ロボットの制御システム１０は、図３に示すように、連続体ロボット１００、連続体ロボット制御装置２００（制御部）、及び、各種の入力装置３１０，３３０〜３４０を有して構成されている。なお、図３では、連続体ロボット１００における外部の構成として、連続体ロボット制御装置２００（制御部）及び入力装置３１０，３３０〜３４０が構成されている例を示しているが、本発明においては、この図３に示す形態に限定されるものではない。例えば、連続体ロボット１００の内部の一構成として、連続体ロボット制御装置２００（制御部）及び入力装置３１０，３３０〜３４０が構成されている形態も、本発明に含まれる。
【００２２】
入力装置３１０は、最遠位湾曲区間の目標湾曲角度θLを連続体ロボット制御装置２００に入力する装置である。具体的に、図１Ａに示す例では、入力装置３１０は、最遠位湾曲区間である湾曲区間１７３の目標湾曲角度θnを、最遠位湾曲区間の目標湾曲角度θLとして、連続体ロボット制御装置２００の後続湾曲角度算出部２１０及び運動学演算部２２０に入力する。なお、入力装置３１０は、最遠位湾曲区間の目標湾曲角度θLに加えて、最遠位湾曲区間の目標旋回角度ζLを更に連続体ロボット制御装置２００に入力するようにしてもよい。
【００２３】
入力装置３３０は、連続体ロボット１００の進行方向（例えば前進する方向）における基台部１４０の変位ｚbを連続体ロボット制御装置２００に入力するとともに、連続体ロボット１００の基台部１４０を変位ｚbだけｚ方向に移動させるべく制御を行う装置である。この制御により、基台部１４０および複数の湾曲区間１７１〜１７３を含む湾曲可能部１７０を一体にして前進移動させることができる。
【００２４】
入力装置３４０は、各種の情報を連続体ロボット制御装置２００に入力する装置である。具体的に、入力装置３４０は、例えば、後続湾曲区間である湾曲区間１７２の長さｌ（湾曲区間１７１の長さを含めてもよい）の情報や、使用する参照テーブル２１１１の指示情報等を、連続体ロボット制御装置２００の情報入力部２１３に入力する。
【００２５】
連続体ロボット制御装置２００は、図３に示すように、後続湾曲角度算出部２１０、及び、運動学演算部２２０を有して構成されている。
【００２６】
後続湾曲角度算出部２１０は、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θLと、後続湾曲区間の目標湾曲角度θFと、入力装置３３０から入力された基台部１４０の変位ｚbと、入力装置３４０から入力された後続湾曲区間の長さｌとに基づいて、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'を算出する算出手段である。この後続湾曲角度算出部２１０は、図３に示すように、マルチプレクサＭｕｘ、記憶部２１１、参照テーブル書換部２１２、及び、情報入力部２１３を有して構成されている。記憶部２１１は、最遠位湾曲区間の目標湾曲角度θL及び後続湾曲区間の目標湾曲角度θFと基台部１４０の変位ｚbとの関係を示す異なる複数の参照テーブル２１１１や、後続湾曲角度算出部２１０において処理に必要な各種の情報を記憶している記憶手段である。情報入力部２１３は、マルチプレクサＭｕｘを介して入力装置３４０から入力された後続湾曲区間の長さｌの情報や使用する参照テーブル２１１１の指示情報等を、参照テーブル書換部２１２に対して入力する情報入力手段である。参照テーブル書換部２１２は、情報入力部２１３から入力された情報に基づき、記憶部２１１に記憶されている複数の参照テーブル２１１１の中から使用する１つの参照テーブル２１１１を選択し、最遠位湾曲区間の目標湾曲角度θLの変更に応じて当該選択した参照テーブル２１１１を書き換える書換手段である。なお、ここで説明した後続湾曲角度算出部２１０の詳細な処理例については、図７等を用いて後述する。
【００２７】
運動学演算部２２０は、後続湾曲角度算出部２１０で算出された変更後の後続湾曲区間の目標湾曲角度θF'に基づいて、駆動部である各アクチュエータで後続湾曲区間のワイヤ（線状部材）を駆動させる際の駆動変位を演算する演算手段である。また、運動学演算部２２０は、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θLに基づいて、駆動部である各アクチュエータで最遠位湾曲区間のワイヤ（線状部材）を駆動させる際の駆動変位を演算する。図３では、運動学演算部２２０の演算により得られた最遠位湾曲区間のワイヤの駆動変位と後続湾曲区間のワイヤの駆動変位をまとめて、ワイヤの駆動変位ｌpとして図示している。そして、本実施形態に係る連続体ロボット１００は、それぞれのワイヤの駆動変位ｌpに応じて、最遠位湾曲区間及び後続湾曲区間の湾曲制御が行われることになる。
【００２８】
本実施形態では、図２に示す、全ての位相角ξn＝０とし、以下には、まず、ｘｚ平面における先頭追従制御について説明する。
【００２９】
１）モデリング
本章では、連続体ロボット１００のｘｚ平面における運動学を導出する。
本章で用いる記号の定義を以下に示す。
ｌn：第ｎの湾曲区間のアーム筐体の長さ
ｒn：ｘｚ平面におけるａワイヤとｂワイヤの距離
ｅ：連続体ロボット１００の湾曲可能部１７０の湾曲区間数
θn：第ｎの湾曲区間の（遠位端の）湾曲角度
ρn：第ｎの湾曲区間の曲率半径
θrefn：第ｎの湾曲区間の（遠位端の）目標湾曲角度
ｌpn：第ｎの湾曲区間のワイヤの駆動変位
ｘtn，ｚtn：第ｎの湾曲区間の遠位端の座標
ｚb：基台部１４０の変位
【００３０】
図４及び図５は、本発明の第１の実施形態を示し、図１Ａに示す連続体ロボット１００の運動学モデルの一例を示す図である。
ここでは、以下の仮定をおき、図４に示す湾曲区間数をｎとする連続体ロボット１００の運動学を導出する。
１．ワイヤは、ｘｚ平面内のみに変形する。
２．各湾曲区間において、ワイヤは曲率一定に変形する。
３．ワイヤのねじり変形は考慮しない。
４．ワイヤは、長手方向に変形しない。
【００３１】
まず、第１の湾曲区間（図１Ａ及び図１Ｂの湾曲区間１７１に相当）のみを考える。
ａワイヤを駆動し、ｂワイヤ及びｃワイヤを固定するとき、ワイヤの駆動変位ｌp1と第１の湾曲区間の（遠位端の）湾曲角度θ1との関係は、以下の（１）式となる。
【数１】
2022002693.tif 000003
【００３２】
次に、第ｎの湾曲区間におけるワイヤの駆動変位ｌpnと、その遠位端の湾曲角度θnとの関係を導出する。ただし、ここでは、ｎは、２以上とする。第ｎの湾曲区間の（遠位端の）相対湾曲角度θ~nを以下のように定義する。
θ~n＝θn−θn-1    ・・・（２）
【００３３】
そして、図５に示すように、原点Ｏの座標を（ｘtn-1，ｚtn-1）とし、θn-1方向及びその直交方向からなる相対座標系ｘn-ｚnをとると、相対座標系ｘn-ｚnにおけるワイヤの駆動変位ｌ~pnと第ｎの湾曲区間の（遠位端の）相対湾曲角度θ~nとの関係は、以下の（３）式となる。ただし、以下の（３）式では、ｌ~pnの「〜」はｌの上方に記載し、また、θ~nの「〜」はθの上方に記載している。
【数２】
2022002693.tif 000004
【００３４】
第ｎの湾曲区間におけるワイヤの駆動変位ｌpnは、第１の湾曲区間から第ｎ−１の湾曲区間までの相対座標系における第ｎの湾曲区間を駆動するためのワイヤの変位の総和となり、以下の（４）式となる。
【数３】
2022002693.tif 000005
【００３５】
これにより、第ｎの湾曲区間の遠位端の湾曲角度θnは、ワイヤの駆動変位ｌpnによってのみ決まり、途中の湾曲区間の角度には依存しないことがわかる。
【００３６】
次に、第ｎ湾曲区間の遠位端の湾曲角度とその遠位端の座標との関係を導出する。まず、第１の湾曲区間を考えると、その第１の湾曲区間の遠位端の湾曲角度θ1とその遠位端の座標（ｘt1，ｚt1）との関係は、以下の（５）式及び（６）式となる。
【数４】
2022002693.tif 000006
【００３７】
次に、第ｎの湾曲区間の遠位端の相対湾曲角度θ~nと相対座標系ｘn-ｚnにおける遠位端の座標（ｘ~tn、ｚ~tn）との関係は、以下の（７）式〜（９）式となる。ここで、ｎは、２以上である。ただし、以下の（７）式及び（８）式では、θ~nの「〜」はθの上方に記載し、また、ｘ~tn及びｚ~tnの「〜」はそれぞれｘ及びｚの上方に記載している。
【数５】
2022002693.tif 000007
【００３８】
これにより、絶対座標系での遠位端の座標（ｘtn，ｚtn）は、回転変換行列を用いて表される。
【００３９】
２）制御系設計
本章では、先頭追従制御系の設計を行う。
図６は、本発明の第１の実施形態に係る連続体ロボット１００の先頭追従制御の一例を示す図である。この図６において、図１Ａに示す構成と同様の構成については同じ符号を付している。また、図６では、図１Ａに示すｚ方向を紙面下側から紙面上側に向かう方向にとっている。また、図６には、基台部１４０及び湾曲可能部１７０を含み構成された連続体ロボット１００が進む目標経路６１０を点線で図示している。
【００４０】
ここで、先頭追従制御とは、図６に示すように、湾曲可能部１７０の最遠位湾曲区間が通る経路（目標経路６１０）と同じ経路を後続の湾曲区間が通るように制御する方法である。先頭追従制御の一例としては、入力された遠位湾曲区間の第１の目標湾曲角度に応じて設定される、連続体ロボット１００の前進移動に対応した追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が第１の目標湾曲角度に至るよう構成されている制御がある。
【００４１】
図６において、時点６０１は、基台部１４０の上面からｚ方向に延びる湾曲可能部１７０が湾曲していない初期状態を示している。その後、図６においては、時点６０２，時点６０３，時点６０４，時点６０５と時間が経過するのにしたがって、基台部１４０がｚ方向に移動して湾曲可能部１７０が湾曲する様子が示されている。
【００４２】
このような先頭追従制御により、連続体ロボット１００は、空間をすり抜けるように進むことができる。なお、先頭追従制御は、予め目標経路６１０が定められていることは必須ではなく、例えば、最遠位湾曲区間の湾曲角度を、後続の湾曲区間の湾曲角度に当該湾曲区間の長さをもって連続的に伝播させてもよい。この方法を用いると、例えば、操縦者は、ジョイステックなどで最遠位湾曲区間の湾曲角度と基台部１４０の変位（移動量（前進量））に対してのみ指令を与えることで、連続体ロボット１００を実時間で先頭追従制御することができる。
【００４３】
図７は、本発明の第１の実施形態を示し、図３の後続湾曲角度算出部２１０による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。図７において、グラフ上の座標は、基台部１４０の位置と湾曲区間（湾曲セクション）の湾曲角度との対を示している。説明のため、以下では、座標ａに対応する湾曲角度のことを"角度ａ"、そして座標ａに対応する基台部１４０の位置（変位）を位置ａ（変位ａ）と呼ぶことがある。なお、以下の図７の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明する。
【００４４】
図７（ａ）において、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図７（ａ）において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θLに対応する、最遠位湾曲区間の湾曲角度のプロファイルを示している。また、図７（ａ）において、太破線の「Follower」は、参照テーブル書換部２１２による書き換えが行われる前の後続湾曲区間の目標湾曲角度θFに対応する、追従湾曲区間の湾曲角度のプロファイルを示している。
【００４５】
具体的には、基台部１４０の変位ｚbが変位ａにおいて、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度ａから最遠位湾曲角度ｂに変更されると、太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFは、基台部１４０の変位ｚbが変位ｃにおいて、後続湾曲角度ｃから後続湾曲角度ｄに変更されるように自動生成される。ここで、変位ｃは、変位ａと変位ｃの間の長さが後続湾曲区間の長さｌとなるように決定される。
【００４６】
しかしながら、図７（ａ）において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルでは、基台部１４０の変位ｚbが変位ａと変位ｃとの間にあるときには、後続湾曲区間の目標湾曲角度θFは変化しない。そして、図７（ａ）において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルでは、変位ｃにおいて後続湾曲角度ｃから後続湾曲角度ｄとなるため、連続体ロボット１００は急峻な挙動を示し、連続体ロボット１００の操作性が損なわれる。そこで、本発明の第１の実施形態では、図７（ａ）において実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、変位ａと変位ｃとの間では、目標湾曲角度ｃと目標湾曲角度ｄと直線で結ぶように補間する。
【００４７】
具体的に、後続湾曲角度算出部２１０の参照テーブル書換部２１２は、以下のようにして、図７（ａ）に示す参照テーブルにおいて太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルを、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルに書き換える。
まず、参照テーブル書換部２１２は、図７（ａ）に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第１の最遠位湾曲角度ａから第２の最遠位湾曲角度ｂに変更されると、第２の最遠位湾曲角度ｂとなった際の基台部１４０の変位である第１変位ａから、第１変位ａから後続湾曲区間の長さｌだけ進んだ変位である第２変位ｃまでを底辺の長さとし（第１変位ａと長さｌの和）、第２の最遠位湾曲角度ｂと第１の最遠位湾曲角度ａとの差を高さとする直角三角形を生成する。具体的には、図７（ａ）で斜線のハッチングを付した直角三角形、即ち、第１変位ａから第２変位ｃまでを底辺の長さとし、第２変位ｃにおける目標湾曲角度ｄと目標湾曲角度ｃとの差を高さとする直角三角形を生成する。
続いて、参照テーブル書換部２１２は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値（具体的には、第１変位ａから第２変位ｃまでの各変位において、基台部１４０の変位ｚbの座標軸（横軸）と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値）を、図７（ａ）において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る値（図７（ａ）に示す例では、０）に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図７（ａ）に示す参照テーブルを書き換える。その後、参照テーブル書換部２１２は、書き換えた参照テーブルを記憶部２１１に記憶する。
【００４８】
図７（ａ）では、最遠位湾曲区間の目標湾曲角度θLにおける一度の変更操作指令に対する後続湾曲区間の目標湾曲角度θFの変更処理方法について図示している。本実施形態では、最遠位湾曲区間の目標湾曲角度θLにおける更なる変更操作指令（複数回の変更操作指令）に対しても、同様のアルゴリズムを用いて後続湾曲区間の目標湾曲角度θFの変更処理を行うことができる。
【００４９】
図７（ｂ）は、図７（ａ）に示す最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令がなされた後に２度目の変更操作指令がなされた場合の例を示している。
【００５０】
図７（ｂ）において、図７（ａ）と同様に、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図７（ｂ）において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図７（ｂ）においては、図７（ａ）において実線の「Interpolated follower」で示す１回目の変更後の後続湾曲区間の目標湾曲角度θF'を、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図７（ｂ）に示す例は、後続湾曲区間の長さｌに満たない基台部１４０の変位ｚb（変位ａから変位ｃまでの範囲内の変位）の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度ｂから最遠位湾曲角度ｂ'に変更された場合を示している。
【００５１】
参照テーブル書換部２１２は、図７（ｂ）に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第１の最遠位湾曲角度ｂから第２の最遠位湾曲角度ｂ'に変更されると、第２の最遠位湾曲角度ｂ'となった際の基台部１４０の変位である第１変位ａ'から、後続湾曲区間の長さｌの和となる変位である第２変位ｃ'までを底辺の長さとし、第２の最遠位湾曲角度ｂ'と第１の最遠位湾曲角度ｂとの差を高さとする直角三角形を生成する。具体的には、図７（ｂ）で斜線のハッチングを付した直角三角形、即ち、第１変位ａ'から第２変位ｃ'までを底辺の長さとし、第２変位ｃ'における目標湾曲角度ｄ'と目標湾曲角度ｃ'との差を高さとする直角三角形を生成する。
続いて、参照テーブル書換部２１２は、生成した直角三角形の斜辺上の点から直角三角形の底辺までの目標湾曲角度に係る値（具体的には、第１変位ａ'から第２変位ｃ'までの各変位において、基台部１４０の変位ｚbの座標軸（横軸）と直交する縦の直線と直角三角形の斜辺との交点を求め、その交点から直角三角形の底辺までの長さの値）を、図７（ｂ）において太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る値に加算し、当該加算後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'として、図７（ｂ）に示す参照テーブルを書き換える。その後、参照テーブル書換部２１２は、書き換えた参照テーブルを記憶部２１１に記憶する。
【００５２】
図７（ｂ）に示すように、最遠位湾曲区間の目標湾曲角度θLにおける２度目の変更操作指令が後続湾曲区間の長さｌに満たない変位ａから変位ｃまでの範囲内において行われると、直線ｅｆの傾きは、直線ａｄの傾きと直線ａ'ｄ'の傾きとの和となる。これは、後続湾曲区間の長さｌを超えない基台部１４０の変位ｚbにおいて、変更操作指令が連続することは操作の修正であると考えると、本実施形態における変更後の後続湾曲区間の目標湾曲角度θF'の算出方法は、以下の特徴を持つ。
・  最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が同方向の場合には、後続湾曲区間は、点ａ'から点ｄまでの変位において、湾曲角度の角速度が増加し、２度目の変更操作指令が強調される湾曲形状に制御される。
・  最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が逆方向の場合には、後続湾曲区間は、点ａ'から点ｄまでの変位において、湾曲角度の角速度が減少し、１度目の変更操作指令が軽減される湾曲形状に制御される。
以上のように、追従湾曲区間（後続湾曲区間）は、第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルに従って、点ｄ'に至るように制御される。
このようにして、第１の湾曲角度のプロファイルとは異なる第２の湾曲角度のプロファイルが設定される。そして、第１の湾曲角度のプロファイルに替えて、第２の湾曲角度のプロファイルに従って追従湾曲区間（後続湾曲区間）の湾曲角度が第２の目標湾曲角度に至るよう構成されている。湾曲角度が目標湾曲角度に至ったかどうかは、使用者が所望の判定基準を設けて判定するとよい。
【００５３】
上述した図７の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明した。そして、連続体ロボット１００の湾曲可能部１７０の湾曲区間数が３つ以上、例えば、図１Ａに示す湾曲可能部１７０の湾曲区間数が３つの場合において、上述した後続湾曲区間として図１Ａの湾曲区間１７１を適用する場合には、例えば、上述した最遠位湾曲区間として１つ前の図１Ａの湾曲区間１７２を適用することで実現可能である。
【００５４】
図８は、本発明の第１の実施形態に係る連続体ロボットの制御システム１０による連続体ロボットの制御方法における処理手順の一例を示すフローチャートである。
まず、ステップＳ８０１において、連続体ロボット制御装置２００は、入力装置３１０から、基台部１４０から最遠位端に位置する湾曲区間である最遠位湾曲区間に対する操作入力を受け付ける。このステップＳ８０１では、連続体ロボット制御装置２００は、入力装置３１０から、例えば最遠位湾曲区間の目標湾曲角度θLの入力を受け付ける。
続いて、ステップＳ８０２において、連続体ロボット制御装置２００は、運動学演算部２２０において、入力された最遠位湾曲区間の目標湾曲角度θLに基づき最遠位湾曲区間のワイヤ（線状部材）の駆動変位を演算し、当該演算結果に基づき連続体ロボット１００を制御する。この制御により、連続体ロボット１００の湾曲可能部１７０の湾曲形状が変更される。
続いて、ステップＳ８０３において、連続体ロボット制御装置２００は、例えば各入力装置３１０，３３０〜３４０からの入力情報に基づいて、連続体ロボット１００に対する操作入力が終了したか否か判断する。
ステップＳ８０３の判断の結果、連続体ロボット１００に対する操作入力が終了していない場合には（Ｓ８０３／Ｎｏ）、ステップＳ８０４に進む。ステップＳ８０４に進むと、連続体ロボット制御装置２００は、例えば入力装置３３０からの入力情報に基づいて、連続体ロボット１００の基台部１４０が前進したか否かを判断する。このステップＳ８０４の判断の結果、連続体ロボット１００の基台部１４０が前進していない場合には（Ｓ８０４／Ｎｏ）、ステップＳ８０１に戻り、ステップＳ８０１以降の処理を再度行う。
一方、ステップＳ８０４の判断の結果、連続体ロボット１００の基台部１４０が前進した場合には（Ｓ８０４／Ｙｅｓ）、ステップＳ８０５に進む。ステップＳ８０５に進むと、後続湾曲角度算出部２１０の参照テーブル書換部２１２は、図７（ａ）または図７（ｂ）に示す参照テーブルにおいて、斜線のハッチングを付した直角三角形データを生成する。図７（ａ）の場合、第１変位ａから第２変位ｃまでの後続湾曲区間の長さｌを底辺の長さ（幅）とし、第２の最遠位湾曲角度θL2に相当する目標湾曲角度ｂと第１の最遠位湾曲角度θL1に相当する目標湾曲角度ａとの差を高さ（より具体的には、第２変位ｃにおける目標湾曲角度ｄと目標湾曲角度ｃとの差を高さ）とする直角三角形を生成する。図７（ｂ）の場合、第１変位ａ'から第２変位ｃ'までの後続湾曲区間の長さｌを底辺の長さ（幅）とし、第２の最遠位湾曲角度θL2に相当する目標湾曲角度ｂ'と第１の最遠位湾曲角度θL1に相当する目標湾曲角度ｂとの差を高さ（より具体的には、第２変位ｃ'における目標湾曲角度ｄ'と目標湾曲角度ｃ'との差を高さ）とする直角三角形を生成する。
続いて、ステップＳ８０６において、後続湾曲角度算出部２１０の参照テーブル書換部２１２は、ステップＳ８０５で生成した直角三角形データを、記憶部２１１に記憶されている現在の参照テーブルの後続湾曲区間の目標湾曲角度θFに加算して補間する。この補間により得られる変更後の後続湾曲区間の目標湾曲角度θF'は、図７（ａ）または図７（ｂ）に示す参照テーブルにおいて実線で示されている。
続いて、ステップＳ８０７において、後続湾曲角度算出部２１０の参照テーブル書換部２１２は、最遠位湾曲区間の目標湾曲角度θLについて、図７（ａ）であれば変位ｃ以降（変位ｃよりも右側）のデータ、図７（ｂ）であれば変位ｃ'以降（変位ｃ'よりも右側）のデータを書き込む処理を行う。このステップＳ８０７の処理が終了すると、ステップＳ８０１に戻る。
また、ステップＳ８０３の判断の結果、連続体ロボット１００に対する操作入力が終了した場合には（Ｓ８０３／Ｙｅｓ）、図８に示すフローチャートの処理を終了する。
【００５５】
３）シミュレーション
本章では、前章の２）制御系設計で説明した先頭追従制御系を用いてシミュレーションを行う。シミュレーションでは、湾曲区間数が２であり、第１の湾曲区間の長さを０．０２ｍとし、第２の湾曲区間の長さを０．０２ｍとした連続体ロボット１００を対象とする。この際、例えば、第２の湾曲区間を上述した最遠位湾曲区間として適用し、第１の湾曲区間を上述した後続湾曲区間として適用する。
【００５６】
図９は、本発明の第１の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図９において、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。また、図９では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
【００５７】
この図９では、連続体ロボット１００の動作開始後にｚ方向に直進状態で０．０２ｍ前進させ、そこで、最遠位湾曲区間を４５度湾曲させる１度目の湾曲変更操作指令がされている。その後、ｚ方向に０．０１ｍ前進させ、そこで、最遠位湾曲区間を同方向にさらに１５度（最終的に６０度）湾曲させる２度目の湾曲変更操作指令がされている。
【００５８】
図１０は、本発明の第１の実施形態を示し、図９に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図１０（ａ）〜図１０（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図１０（ａ）〜図１０（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００５９】
図１０（ａ）では、連続体ロボット１００をｚ方向に直進状態で０．０２ｍまで前進させ、そこで、湾曲可能部１７０の最遠位湾曲区間を４５度に湾曲させている。続いて、図１０（ｂ）では、目標経路６１０との誤差を修正するために、連続体ロボット１００の前進をやめ、続く、図１０（ｃ）において、湾曲可能部１７０の最遠位湾曲区間を６０度まで湾曲させている。この２度目の湾曲操作は、１度目の湾曲操作から、湾曲区間の長さ（後続湾曲区間の長さｌ＝０．０２ｍ）を超えない基台部１４０の変位Ｚbにおいて行われている。そのため、図９に示すように、実線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルは、基台部１４０の変位ｚbが０．０３ｍから０．０４ｍまでの間は角速度が大きく、湾曲操作の修正がその後の短い基台部１４０の変位ｚbにおいて後続湾曲区間に反映されるようになっている。これにより、図１０（ｄ）において、湾曲可能部１７０を目標経路６１０に接近させることができる。
【００６０】
図１１は、本発明の第１の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図１１において、図９で説明した事項と同様の事項については説明を省略する。
【００６１】
この図１１では、連続体ロボット１００の動作開始後にｚ方向に直進状態で０．０２ｍ前進させ、そこで、最遠位湾曲区間を４５度湾曲させる１度目の湾曲変更操作指令がされている。その後、ｚ方向に０．０１ｍ前進させ、そこで、最遠位湾曲区間を逆方向に２０度（最終的に２５度）湾曲させる２度目の湾曲変更操作指令がされている。
【００６２】
図１２は、本発明の第１の実施形態を示し、図１１に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図１２（ａ）〜図１２（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図１０（ａ）〜図１０（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００６３】
図１２（ａ）では、連続体ロボット１００をｚ方向に直進状態で０．０２ｍまで前進させ、そこで、湾曲可能部１７０の最遠位湾曲区間を４５度に湾曲させている。続いて、図１２（ｂ）では、目標経路６１０との誤差を修正するために、連続体ロボット１００の前進をやめ、図１２（ｃ）において、湾曲可能部１７０の最遠位湾曲区間を２５度まで湾曲を浅くしている。そのため、図１１に示すように、実線の「Follower」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルは、基台部１４０の変位ｚbが０．０３ｍから０．０４ｍまでの間は角速度が小さく、後続湾曲区間の目標湾曲角度θFが４５度に到達することは取り消され、湾曲操作の修正がその後の短い基台部１４０の変位ｚbにおいて後続湾曲区間に反映されるようになっている。これにより、図１２（ｄ）において、湾曲可能部１７０を目標経路６１０に接近させることができる。
【００６４】
以上説明した第１の実施形態に係る連続体ロボットの制御システム１０では、図７等に示すように、最遠位湾曲区間の目標湾曲角度θLと、基台部１４０の変位ｚbと、後続湾曲区間の長さｌとに基づいて、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'を算出するようにしている。
かかる構成によれば、湾曲区間（本実施形態では、後続湾曲区間）の長さに満たない基台部１４０の変位ｚbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット１００の操作性の向上を図ることができる。これにより、連続体ロボット１００を目標経路６１０に沿って動作させる対象物を傷つけたり、また、連続体ロボット１００を破損させたりする不具合を抑制、あるいは回避することが可能となる。
【００６５】
（第２の実施形態）
次に、本発明の第２の実施形態について説明する。なお、以下に記載する第２の実施形態の説明では、上述した第１の実施形態と共通する事項については説明を省略し、上述した第１の実施形態と異なる事項について説明を行う。
【００６６】
第２の実施形態に係る連続体ロボットの概略構成は、図１Ａ及び図１Ｂに示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第２の実施形態に係る連続体ロボットの制御システムの概略構成は、図３に示す第１の実施形態に係る連続体ロボットの制御システム１０の概略構成と同様である。
【００６７】
上述した第１の実施形態では、湾曲可能部１７０が目標経路６１０に接近するように後続湾曲区間の目標湾曲角度を設定する形態であった。第２の実施形態では、最遠位湾曲区間の目標湾曲角度に忠実に後続湾曲区間の目標湾曲角度を設定する形態について説明する。
【００６８】
図１３は、本発明の第２の実施形態を示し、図３の後続湾曲角度算出部２１０による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。なお、以下の図１３の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明する。また、図１３は、図７（ａ）に示す最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令がなされた後に２度目の変更操作指令がなされた場合の例を示しているものとする。
【００６９】
図１３において、図７（ｂ）と同様に、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。図１３において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図１３においては、図７（ａ）において実線の「Interpolated follower」で示す１回目の変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図１３に示す例は、後続湾曲区間の長さｌに満たない基台部１４０の変位ｚb（変位ａから変位ｃまでの範囲内の変位）の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度ｂから最遠位湾曲角度ｂ'に変更された場合を示している。
【００７０】
参照テーブル書換部２１２は、図１３に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第１の最遠位湾曲角度ｂから第２の最遠位湾曲角度ｂ'に変更されると、第２の最遠位湾曲角度ｂ'となった際の基台部１４０の変位ｚbである変位ａ'から、後続湾曲区間の長さｌの和となる変位である第２変位ｃ'までのうち、第１の最遠位湾曲角度ｂとなった際の基台部１４０の変位ａから、後続湾曲区間の長さｌの和となる変位である第１変位ｄまでを除いた変位範囲（第１変位ｄから第２変位ｃ'までの範囲）について、図１３に示す太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルを、第１変位ｄにおける第１の最遠位湾曲角度ｂに係る値と第２変位ｃ'における第２の最遠位湾曲角度ｂ'に係る値とを結んだ直線上の目標湾曲角度に係る値で補間する。そして、当該補間後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、図１３に示す参照テーブルを書き換える。その後、参照テーブル書換部２１２は、書き換えた参照テーブルを記憶部２１１に記憶する。
【００７１】
このように、本実施形態の制御系では、操縦者等による最遠位湾曲区間に対する湾曲操作の全てが補間されて後続湾曲区間へと伝播される。
【００７２】
上述した図１３の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明した。そして、連続体ロボット１００の湾曲可能部１７０の湾曲区間数が３つ以上、例えば、図１Ａに示す湾曲可能部１７０の湾曲区間数が３つの場合において、上述した後続湾曲区間として図１Ａの湾曲区間１７１を適用する場合には、例えば、上述した最遠位湾曲区間として１つ前の図１Ａの湾曲区間１７２を適用することで実現可能である。
【００７３】
図１４は、本発明の第２の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルにおける１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の湾曲角度に係る湾曲角度のプロファイルにおけるシミュレーション結果の一例を示す図である。この図１４において、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。また、図１４では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
【００７４】
図１５は、本発明の第２の実施形態を示し、図１４に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図１５（ａ）〜図１５（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図１５（ａ）〜図１５（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００７５】
図１５（ａ）〜図１５（ｃ）の応答は、それぞれ、上述した第１の実施形態における図１０（ａ）〜図１０（ｃ）の応答と等しい。図１５（ｄ）において、第２の実施形態の制御方法では、操縦者等による湾曲操作を補間しそのまま後続湾曲区間に伝播させているので、第１の実施形態で示したような後続湾曲区間の角速度の急変動は起こらない。そのため、湾曲可能部１７０の目標経路６１０の接近は緩やかなものとなる。
【００７６】
図１６は、本発明の第２の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図１６において、図１４で説明した事項と同様の事項については説明を省略する。
【００７７】
図１７は、本発明の第２の実施形態を示し、図１６に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図１７（ａ）〜図１７（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図１７（ａ）〜図１７（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００７８】
図１７（ａ）〜図１７（ｃ）の応答は、それぞれ、上述した第１の実施形態における図１２（ａ）〜図１２（ｃ）の応答と等しい。図１７（ｄ）において、第２の実施形態の制御方法では、第１の実施形態で示したような後続湾曲区間への操作量が低減され伝播されることはない。そのため、湾曲可能部１７０の目標経路６１０への接近は、急峻なものとなり、オーバーシュートしうるが、最遠位湾曲区間の操作量そのものに対しては忠実に伝播されている。
【００７９】
第２の実施形態においても、上述した第１の実施形態と同様に、後続湾曲区間の長さに満たない基台部１４０の変位ｚbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット１００の操作性の向上を図ることができる。これにより、連続体ロボット１００を目標経路６１０に沿って動作させる対象物を傷つけたり、また、連続体ロボット１００を破損させたりする不具合を回避することが可能となる。
【００８０】
（第３の実施形態）
次に、本発明の第３の実施形態について説明する。なお、以下に記載する第３の実施形態の説明では、上述した第１及び第２の実施形態と共通する事項については説明を省略し、上述した第１及び第２の実施形態と異なる事項について説明を行う。
【００８１】
第３の実施形態に係る連続体ロボットの概略構成は、図１Ａ及び図１Ｂに示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第３の実施形態に係る連続体ロボットの制御システムの概略構成は、図３に示す第１の実施形態に係る連続体ロボットの制御システム１０の概略構成と同様である。
【００８２】
上述した第２の実施形態では、最遠位湾曲区間の目標湾曲角度に忠実に後続湾曲区間の目標湾曲角度を設定する形態であった。第３の実施形態では、湾曲区間の長さｌを超えない基台部１４０の変位ｚbにおける最遠位湾曲区間に対する湾曲操作を取り消し、後続湾曲区間に伝播させる形態について説明する。
【００８３】
図１８は、本発明の第３の実施形態を示し、図３の後続湾曲角度算出部２１０による最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'の算出処理の一例を示す図である。なお、以下の図１８の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明する。また、図１８は、図７（ａ）に示す最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令がなされた後に２度目の変更操作指令がなされた場合の例を示しているものとする。
【００８４】
図１８において、図７（ｂ）と同様に、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。図１８において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示している。また、図１８においては、図７（ａ）において実線の「Interpolated follower」で示す１回目の変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを、太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFに係る湾曲角度のプロファイルとして示している。この図１８に示す例は、後続湾曲区間の長さｌに満たない基台部１４０の変位ｚb（変位ａから変位ｃまでの範囲内の変位）の際に、最遠位湾曲区間の目標湾曲角度θLが最遠位湾曲角度ｂから最遠位湾曲角度ｂ'に変更された場合を示している。
【００８５】
参照テーブル書換部２１２は、図１８に示す参照テーブルにおいて、最遠位湾曲区間の目標湾曲角度θLが第１の最遠位湾曲角度ｂから第２の最遠位湾曲角度ｂ'に変更されると、第２の最遠位湾曲角度ｂ'となった際の基台部１４０の変位ｚbである第１変位ａ'から、後続湾曲区間の長さｌの和となる変位である第２変位ｃ'までの変位範囲について、図１８に示す太い一点鎖線の「First Interpolation」で示す後続湾曲区間の目標湾曲角度θFを、第１変位ａ'における後続湾曲区間の目標湾曲角度θFに係る値ｅと第２変位ｃ'における第２の最遠位湾曲角度ｂ'に係る値とを結んだ直線上の目標湾曲角度に係る値で補間する。そして、当該補間後の後続湾曲区間の目標湾曲角度を、実線の「Interpolated follower」で示す変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルとして、図１８に示す参照テーブルを書き換える。
【００８６】
図１８に示すように、第３の実施形態では、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令を後続湾曲区間に伝播するための点ｄは取り消される。これにより、湾曲区間の長さｌを超えない基台部１４０の変位ｚbにおいて湾曲操作が連続したとき、最終の湾曲操作時の後続湾曲区間の湾曲角度と最終の湾曲操作による目標湾曲角度が線形補間されて後続湾曲区間に伝播される。
【００８７】
上述した図１８の説明では、最遠位湾曲区間として図１Ａの湾曲区間１７３を適用し、後続湾曲区間として図１Ａの湾曲区間１７２を適用した場合を想定した例について説明した。そして、連続体ロボット１００の湾曲可能部１７０の湾曲区間数が３つ以上、例えば、図１Ａに示す湾曲可能部１７０の湾曲区間数が３つの場合において、上述した後続湾曲区間として図１Ａの湾曲区間１７１を適用する場合には、例えば、上述した最遠位湾曲区間として１つ前の図１Ａの湾曲区間１７２を適用することで実現可能である。
【００８８】
図１９は、本発明の第３の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が同方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図１９において、横軸は、基台部１４０の変位ｚbを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標湾曲角度θを示している。また、図１９では、破線の「Leader」は、最遠位湾曲区間の目標湾曲角度θLに係る湾曲角度のプロファイルを示し、実線の「Follower」は、最遠位湾曲区間の目標湾曲角度θLの変更に伴う変更後の後続湾曲区間の目標湾曲角度θF'に係る湾曲角度のプロファイルを示している。
【００８９】
図２０は、本発明の第３の実施形態を示し、図１９に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図２０（ａ）〜図２０（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図２０（ａ）〜図２０（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００９０】
図２０（ａ）〜図２０（ｃ）の応答は、それぞれ、上述した第１の実施形態における図１０（ａ）〜図１０（ｃ）の応答と等しい。図２０（ｄ）において、第３の実施形態の制御方法では、最終の湾曲操作時の後続湾曲区間の湾曲角度と、遠位湾曲区間に対する最終の湾曲操作の角度が線形補間されて目標湾曲角度が算出されるため、第１の実施形態で示したような後続湾曲区間の角速度の変動は起こらない。そのため、湾曲可能部１７０の目標経路６１０の接近は緩やかなものとなる。
【００９１】
図２１は、本発明の第３の実施形態を示し、最遠位湾曲区間の目標湾曲角度θLにおける１度目の変更操作指令と２度目の変更操作指令が逆方向の場合の後続湾曲区間の目標湾曲角度におけるシミュレーション結果の一例を示す図である。この図２１において、図１９で説明した事項と同様の事項については説明を省略する。
【００９２】
図２２は、本発明の第３の実施形態を示し、図２１に示すシミュレーション結果を反映した連続体ロボット１００の湾曲可能部１７０における動作状態の一例を示す図である。具体的に、図２２（ａ）〜図２２（ｄ）は、時系列ごとに連続体ロボット１００の湾曲可能部１７０における動作状態を示したものであり、横軸はｘ方向の変位を示し、縦軸はｚ方向の変位を示している。また、図２２（ａ）〜図２２（ｄ）において、実線は、連続体ロボット１００の湾曲可能部１７０の湾曲状態を示し、破線は、目標経路６１０を示している。
【００９３】
図２２（ａ）〜図２２（ｃ）の応答は、それぞれ、上述した第１の実施形態における図１２（ａ）〜図１２（ｃ）の応答と等しい。図２２（ｄ）において、第３の実施形態の制御方法では、湾曲区間の長さｌを超えない基台部１４０の変位ｚbにおける修正湾曲操作は伝播されない。そのため、湾曲可能部１７０の目標経路６１０への追従性は緩やかなものとなるが、第２の実施形態に示したように後続湾曲区間の湾曲により目標経路６１０をオーバーシュートすることはない。
【００９４】
第３の実施形態においても、上述した第１の実施形態と同様に、後続湾曲区間の長さに満たない基台部１４０の変位ｚbの際に最遠位湾曲区間の目標湾曲角度θLが変更された場合においても、連続体ロボット１００の操作性の向上を図ることができる。これにより、連続体ロボット１００を目標経路６１０に沿って動作させる対象物を傷つけたり、また、連続体ロボット１００を破損させたりする不具合を回避することが可能となる。
【００９５】
（第４の実施形態）
次に、本発明の第４の実施形態について説明する。なお、以下に記載する第４の実施形態の説明では、上述した第１〜第３の実施形態と共通する事項については説明を省略し、上述した第１〜第３の実施形態と異なる事項について説明を行う。
【００９６】
上述した第１〜第３の実施形態では、ｘｚ平面内での連続体ロボット１００の先頭追従制御法について説明した。第４の実施形態では、図１Ａ及び図１Ｂに示すｘｙｚの３次元空間での先頭追従制御を行う。
【００９７】
連続体ロボット１００の湾曲角度及び旋回角度を制御するためのアクチュエータによる駆動変位を求めるため、運動学の導出を行う。
本実施形態に用いる記号の定義を以下に示す。
ｌd：湾曲区間の中心軸の長さ
θn：遠位湾曲区間の湾曲角度
ζn：遠位湾曲区間の旋回角度
ρn：湾曲区間の曲率半径
ζrefn：第ｎの湾曲区間の遠位端の目標旋回角度
【００９８】
ここでは、以下の仮定をおき、連続体ロボット１００の運動学を導出する。
１．各湾曲区間において、ワイヤは曲率一定に変形する。
２．ワイヤのねじり変形は考慮しない。
３．ワイヤは、長手方向に変形しない。
４．ワイヤガイドとワイヤと間の摩擦は考慮しない。
【００９９】
まず、第１の湾曲区間（図１Ａ及び図１Ｂの湾曲区間１７１に相当）のａワイヤ、ｂワイヤ及びｃワイヤの駆動変位ｌp1a，ｌp1b及びｌp1cと、その遠位端の湾曲角度θ1、並びに、旋回角度ζ1の関係は、以下の（１０）式となる。
【数６】
2022002693.tif 000008
【０１００】
次に、複数の湾曲区間を有する連続体ロボット１００のａワイヤ、ｂワイヤ及びｃワイヤの駆動変位ｌpna，ｌpnb及びｌpncと、その遠位端の湾曲角度θn、並びに、旋回角度ζnとの関係を求める。湾曲区間数をｅとし、第ｎの湾曲区間を駆動するワイヤの位相角を以下の（１１）式とする。
【数７】
2022002693.tif 000009
【０１０１】
これにより、第ｎの湾曲区間のワイヤ駆動変位ｌpna，ｌpnb及びｌpncは、以下の（１２）式となる。
【数８】
2022002693.tif 000010
【０１０２】
操縦者によって入力装置３１０を介して最遠位端の第ｅの湾曲区間に対して、目標湾曲角度θrefe及び目標旋回角度ζrefeが与えられたとき、（１２）式を用いてワイヤ駆動変位を求めることで、最遠位端の角度を制御することができる。そして、先頭追従制御は平面駆動の第１〜第３の実施形態と同様に、目標湾曲角度を伝播させればよく、第１〜第３の実施形態の湾曲角度を旋回角度と読み替えて同様のアルゴリズムにより目標角度を算出すればよい。そして、後続湾曲区間の各々に対して、（１２）式を用いてワイヤ駆動変位を求めることで、３次元空間内での先頭追従制御を行うことができる。
【０１０３】
（その他の実施形態）
その他の実施形態として、記憶部２１１は、上述した第１〜第３の実施形態に係る先頭追従制御法に用いる参照テーブル２１１１をそれぞれ記憶しておき、参照テーブル書換部２１２は、情報入力部２１３からの入力情報に基づき、使用する実施形態の参照テーブル２１１１を選択して処理するようにしてもよい。
【０１０４】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
【０１０５】
なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。即ち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。
【符号の説明】
【０１０６】
１０：連続体ロボットの制御システム、１００：連続体ロボット、１４０：基台部、１７０：湾曲可能部、１７１〜１７３：湾曲区間、２００：連続体ロボット制御装置、２１０：後続湾曲角度算出部、２１１：記憶部、２１１１：参照テーブル、２１２：参照テーブル書換部、２１３：情報入力部、２２０：運動学演算部、３１０〜３４０：入力装置
【図１Ａ】
2022002693.tif 000011
【図１Ｂ】
2022002693.tif 000012
【図２】
2022002693.tif 000013
【図３】
2022002693.tif 000014
【図４】
2022002693.tif 000015
【図５】
2022002693.tif 000016
【図６】
2022002693.tif 000017
【図７】
2022002693.tif 000018
【図８】
2022002693.tif 000019
【図９】
2022002693.tif 000020
【図１０】
2022002693.tif 000021
【図１１】
2022002693.tif 000022
【図１２】
2022002693.tif 000023
【図１３】
2022002693.tif 000024
【図１４】
2022002693.tif 000025
【図１５】
2022002693.tif 000026
【図１６】
2022002693.tif 000027
【図１７】
2022002693.tif 000028
【図１８】
2022002693.tif 000029
【図１９】
2022002693.tif 000030
【図２０】
2022002693.tif 000031
【図２１】
2022002693.tif 000032
【図２２】
2022002693.tif 000033