(57)【要約】【課題】連続体ロボットが目標経路から逸脱しないようにする。【解決手段】連続体ロボットシステムの制御装置は、長尺方向に直列に設けられた湾曲可能な複数の湾曲部１１１，１１２を有しそれらの長尺方向に移動可能な連続体ロボット１と、連続体ロボット１を長尺方向に移動させる駆動ユニット２と、複数の湾曲部１１１，１１２のそれぞれについて先位端角度θ1，θ2を変更する複数の角度制御モータ２１１，２１２と、追従可能速度を計算するとともに駆動ユニット２を制御する駆動ユニット速度計算・制御部４４とを有し、駆動ユニット速度計算・制御部４４は、追従可能速度以下の速度で前記連続体ロボットを移動させるように駆動ユニット２を制御する。【選択図】図５
2018175602.tif 000002
【特許請求の範囲】
【請求項１】
長尺方向に直列に設けられた、それぞれが湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、
前記複数の湾曲部のそれぞれを湾曲させることによりそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御装置であって、
前記制御装置は、
前記移動駆動部を制御する移動駆動制御手段と、
前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
を有し、
前記移動駆動制御手段は、前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下にすることを特徴とする連続体ロボットシステムの制御装置。
【請求項２】
前記移動駆動制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記移動速度を、計算した前記最大値のうちで最も小さい値にすることを特徴とする請求項１に記載の連続体ロボットシステムの制御装置。
【請求項３】
前記移動駆動制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記移動速度を、計算した前記最大値のうちで最も小さい値に１以下の値の係数を乗じた値にすることを特徴とする請求項２に記載の連続体ロボットシステムの制御装置。
【請求項４】
前記移動駆動制御手段は、前記移動速度を、閾値以上の速度にすることを制御することを特徴とする請求項１から３のいずれか１項に記載の連続体ロボットシステムの制御装置。
【請求項５】
長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記連続体ロボットを前記長尺方向の移動速度を減速させるブレーキ部と、
前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御装置であって、
前記制御装置は、
前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
前記ブレーキ部を制御するブレーキ制御手段と、
を有し、
前記ブレーキ制御手段は、前記連続体ロボットの前記長尺方向の移動速度が前記最大値を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させることを特徴とする連続体ロボットシステムの制御装置。
【請求項６】
前記ブレーキ制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記連続体ロボットの前記長尺方向の移動速度が前記最大値のうちで最も小さい移動速度を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させることを特徴とする請求項５に記載の連続体ロボットシステムの制御装置。
【請求項７】
長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御装置であって、
前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下に制限する移動速度制限手段と、
を有することを特徴とする連続体ロボットシステムの制御装置。
【請求項８】
前記姿勢変更部は、前記複数の湾曲部の長尺方向の一方の端部に設けられており、
前記複数の湾曲部のそれぞれの前記姿勢は、前記複数の湾曲部のそれぞれについて、中心線に直角であり前記一方の端部とは反対側の端部を通過する直線と、移動方向に直角であり前記複数の湾曲部のうちの前記姿勢変更部に最も近い湾曲部の前記姿勢変更部に近い側の端部を通過する直線とがなす角度であることを特徴とする請求項１から７のいずれか１項に記載の連続体ロボットシステムの制御装置。
【請求項９】
長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、
前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御方法であって、
前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を前記最大値以下に設定するステップと、
を有することを特徴とする連続体ロボットシステムの制御方法。
【請求項１０】
長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記連続体ロボットを前記長尺方向の移動速度を減速させるブレーキ部と、
前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御方法であって、
前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
前記連続体ロボットの前記長尺方向の移動速度が前記最大値を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させるステップと、
を有することを特徴とする連続体ロボットシステムの制御方法。
【請求項１１】
長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
を有する連続体ロボットシステムの制御方法であって、
前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下に制限する移動速度制限ステップと、
を有することを特徴とする連続体ロボットシステムの制御方法。
【請求項１２】
コンピュータを、請求項１から８のいずれか１項に記載の制御装置として機能させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、連続体ロボットシステムの制御装置、連続体ロボットシステムの制御方法、プログラムに関する。
【背景技術】
【０００２】
特許文献１には、連続体ロボットの湾曲部の姿勢を制御する制御器を備え、制御器が連続体ロボットの進行方向の先頭に位置する湾曲部が通過した経路に沿うように後続の湾曲部を制御する内視鏡が開示されている。以下、このような姿勢制御を「先頭追従制御」と称する。先頭追従制御を行う制御器を有する内視鏡によれば、内視鏡の挿入対象である被検者の体腔への挿入時には、使用者は挿入方向の先頭に位置する湾曲部の姿勢を操作するだけで、後続する湾曲部は、被検者の体組織と接触しないように自動的に制御される。また、抜去時においては、各湾曲部の姿勢は挿入時に通過した経路に沿うように自動的に制御されるため、使用者は湾曲部の姿勢を操作しなくてもよい。このように、先頭追従制御を用いることによって、連続体ロボットの挿入・抜去時における使用者の負担を軽減することができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】米国特許出願公開第２００５／０２０９５０９号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００４】
先頭追従制御では、目標経路に沿うように連続体ロボットの湾曲部の姿勢を変化させることから、高速に挿入・抜去動作を行うためには、湾曲部の姿勢を高速に変化させる必要がある。しかしながら、一般に連続体ロボットの姿勢を変化させるアクチュエータや駆動伝達機構の応答速度や駆動速度には上限が存在する。このため、アクチュエータに駆動指令を与えてから（アクチュエータが動作開始してから）湾曲部の姿勢が目標姿勢と一致するまでには、ある程度の時間を要する。そのため、連続体ロボットを高速で挿入・抜去させると、湾曲部の姿勢の変化が目標経路の形状に追従できず、アクチュエータが目標経路から逸脱するおそれがある。
【０００５】
本発明が解決しようとする課題は、連続体ロボットを前進や後進させる際に、連続体ロボットが目標経路から逸脱しないようにすることである。
【課題を解決するための手段】
【０００６】
本発明は、長尺方向に直列に設けられた、それぞれが湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、前記複数の湾曲部のそれぞれを湾曲させることによりそれぞれの姿勢を変更する姿勢変更部と、を有する連続体ロボットシステムの制御装置であって、前記制御装置は、前記移動駆動部を制御する移動駆動制御手段と、前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、を有し、前記移動駆動制御手段は、前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下にすることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、連続体ロボットを前進動作や後進動作させる際に、連続体ロボットが目標経路から逸脱しないようにできる。
【図面の簡単な説明】
【０００８】
【図１】第１の実施形態に係る連続体ロボットシステムの構成例を示す模式図である。
【図２】連続体ロボットの構成例を示す模式図である。
【図３】連続体ロボットの第１湾曲部のモデルを示す模式図である。
【図４】連続体ロボットの第２湾曲部のモデルを説明する模式図である。
【図５】制御装置の構成例を示すブロック図である。
【図６】連続体ロボットの先頭追従制御の例を示す図である。
【図７】制御装置が実行する処理の例を示すフローチャートである。
【図８】第２の実施形態に係る連続体ロボットシステムの構成例を模式的に示す図である。
【図９】本実施形態に係る制御装置の構成例を示すブロック図である。
【図１０】制御装置が実行する処理の例を示すフローチャートである。
【図１１】制御装置が実行する処理の例を示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下に、本発明の各実施形態について、図面を参照して詳細に説明する。本発明の実施形態では、連続体ロボットシステムが軟性内視鏡に適用される例を示す。近年、患者の負担を低減し治療・検査後のＱＯＬを向上させるための低侵襲医療が注目を集めており、低侵襲医療の例として、内視鏡を用いた手術・検査が挙げられる。例えば、腹腔鏡手術は従来の開腹手術と比べて手術創を小さくすることが可能となるため、術後に必要な入院期間を短縮できるだけでなく、美容上も優れているというメリットがある。低侵襲医療に用いられる内視鏡は硬性内視鏡と軟性内視鏡に大別される。このうち、軟性内視鏡は、挿入部が操作者の操作により湾曲可能な部材で構成されているため、内視鏡先端の湾曲角度を操作することで広い範囲をくまなく観察できる。
【００１０】
＜第１の実施形態＞
（連続体ロボットシステムの構成の例）
まず、第１の実施形態に係る連続体ロボットシステムの構成例について説明する。図１は、第１の実施形態に係る連続体ロボットシステムの構成例を示す模式図である。なお、図１（ａ）は、連続体ロボットシステムの全体構成を示し、図１（ｂ）は連続体ロボット１と駆動ユニット２とリニアガイド３と移動駆動部３２とを示す。図１に示すように、第１の実施形態に係る連続体ロボットシステムは、連続体ロボット１と、駆動ユニット２と、リニアガイド３と、移動駆動部３２と、制御装置４と、角度入力部５と、ガイド操作部７と、カメラ６とを含んでいる。ただし、連続体ロボットシステムに含まれる機器の種類や構成は、図１に示す例に限定されるものではない。
【００１１】
連続体ロボット１は、長尺方向に直列に連続するように設けられている複数の湾曲部を有している。複数の湾曲部のそれぞれは、円弧状に湾曲可能である。そして、それぞれの湾曲部は、駆動ユニット２に設けられている複数の姿勢変更部（アクチュエータ）のそれぞれによって変形駆動されて姿勢が変化する。なお、連続体ロボット１の詳細な構成例については後述する。本実施形態では、直列に連続するように設けられている複数の湾曲部のうち、駆動ユニット２から最も遠い湾曲部を「遠位端湾曲部」と称し、遠位端湾曲部の反対側であって駆動ユニット２に最も近い湾曲部を「近位端湾曲部」と称する。連続体ロボット１の全体およびそれぞれの湾曲部についても、長尺方向について駆動ユニットから遠い側の端部を「遠位端」と称し、遠位端の反対側の端部であって駆動ユニット２に近い側の端部を「近位端」と称する。連続体ロボット１は、長尺方向に移動可能であり、移動駆動部３２の駆動力によって、駆動ユニット２とともに前後進動作する。本実施形態では、遠位端湾曲部を先頭とする長尺方向の移動を前進動作とし、近位端湾曲部を先頭とする長尺方向の移動を後進動作とする。
【００１２】
遠位端湾曲部の遠位端には、カメラ６が設けられている。なお、カメラ６の構成は特に限定されるものではなく、軟性内視鏡に用いられている公知の各種カメラが適用できる。
【００１３】
駆動ユニット２は、リニアガイド３の長尺方向に往復移動可能である。移動駆動部３２は、制御装置４の制御にしたがって駆動ユニット２をリニアガイド３に沿って前後進動作させる。そして、駆動ユニット２が移動駆動部３２の駆動力によって前後進動作（本例では、直線移動）することにより、連続体ロボット１がその長尺方向に前後進動作する。なお、リニアガイド３には長尺方向の位置を示すスケール３１が設けられており、駆動ユニット２にはスケール３１を検出するセンサ２４と図略の速度・位置計算部が設けられている。駆動ユニット２の速度・位置計算部は、センサ２４によるスケール３１の検出結果から駆動ユニット２の前後進動作の移動速度と現在位置を計算し、制御装置４に出力（送信）する。なお、移動駆動部３２の構成は特に限定されるものではなく、例えば公知の各種リニアアクチュエータなどが適用できる。
【００１４】
ガイド操作部７は、連続体ロボットシステムの使用者（操作者）が操作する機器である。ガイド操作部７には、制御装置４に前後進動作の指令を送信するための操作部材である前後進ボタン７１と、駆動ユニット２の前後進動作の要求移動速度ｖrefを入力（設定）するためのボリューム７２とが設けられている。「要求移動速度」とは、使用者が要求する連続体ロボット１（駆動ユニット２）の前後進動作の移動速度をいう。ガイド操作部７は、前後進ボタン７１の操作に応じて駆動ユニット２の前後進動作の指令を移動駆動部３２に出力し、ボリューム７２の操作に応じて要求移動速度ｖrefを移動駆動部３２に出力する。なお、ガイド操作部７の具体的な構成は特に限定されるものではない。ガイド操作部７は、使用者が移動駆動部３２に駆動ユニット２の前後進動作を指令するための操作と、駆動ユニット２の要求移動速度ｖrefの入力（設定）操作とができる構成であればよい。
【００１５】
角度入力部５は、使用者が連続体ロボット１の遠位端湾曲部の遠位端角度を入力（設定）するための機器である。「遠位端湾曲部の遠位端角度」とは、連続体ロボット１が姿勢変化（角度変化）する平面上において、「遠位端湾曲部の遠位端を通過しその中心線に直角な直線」と「近位端湾曲部の近位端を通過しその前後進動作の移動方向に直角な直線」とがなす角度をいうものとする。角度入力部５には、例えば、遠位端湾曲部の遠位端角度を入力（設定）するための操作部材であるレバー５１が設けられている。使用者はレバー５１の操作により遠位端湾曲部の遠位端角度を入力（設定）でき、入力された遠位端湾曲部の遠位端角度は制御装置４に出力（送信）される。なお、角度入力部５の具体的な構成は特に限定されるものではない。角度入力部５は、遠位端湾曲部の目標姿勢である遠位端角度を入力でき、入力された遠位端角度を制御装置４に送信できる構成であればよい。
【００１６】
制御装置４は、ガイド操作部７から取得した前後進動作の指令および要求移動速度ｖrefと、角度入力部５から取得した遠位端湾曲部の遠位端角度と、駆動ユニット２の速度・位置計算部による位置および速度の計算結果とに基づいて、駆動ユニット２を駆動する。なお、制御装置４の構成例および制御の例については後述する。
【００１７】
（連続体ロボットの構成例）
次に、連続体ロボット１の構成例について説明する。図２は、連続体ロボット１の構成例を示す模式図である。本実施形態に係る連続体ロボット１は、複数の湾曲部の例として、第１湾曲部１１１と第２湾曲部１１２との２つの湾曲部を有している。本実施形態では、第１湾曲部１１１が近位端湾曲部であるものとし、第２湾曲部１１２が遠位端湾曲部であるものとする。第１湾曲部１１１と第２湾曲部１１２は、円弧状に湾曲可能であり、互いに独立して姿勢の変更が可能である。
【００１８】
第１湾曲部１１１の遠位端には第１ワイヤ固定部２６１が設けられており、この第１ワイヤ固定部２６１には、駆動伝達機構である第１ワイヤ２３１の一方の端部が固定されている。第１ワイヤ２３１は、第１湾曲部１１１に沿って設けられている。そして、第１ワイヤ２３１の他方の端部は、駆動ユニット２の内部に引き込まれており、駆動ユニット２の内部に設けられている第１プーリ２２１に連結されている。第１プーリ２２１は、駆動ユニット２の内部に設けられている姿勢変更部の例である第１角度制御モータ２１１の駆動力によって回転する。本実施形態では、第１プーリ２２１と第１角度制御モータ２１１の出力軸２１３とに第１ベルト２１４が巻き掛けられており、第１角度制御モータ２１１の出力軸２１３の回転がこの第１ベルト２１４を介して第１プーリ２２１に伝達されるように構成されている。
【００１９】
同様に、第２湾曲部１１２の遠位端には第２ワイヤ固定部２６２が設けられており、第２ワイヤ固定部２６２には、駆動力伝達機構である第２ワイヤ２３２の一方の端部が固定されている。第２ワイヤ２３２は、第１湾曲部１１１と第２湾曲部１１２に沿って設けられており、長尺方向の中間部において第１ワイヤ固定部２６１に設けられている孔を通過している。そして、第２ワイヤ２３２の他方の端部は、駆動ユニット２の内部に引き込まれており、駆動ユニット２の内部に設けられている第２プーリ２２２に連結されている。第２プーリ２２２は、駆動ユニット２の内部に設けられている姿勢変更部の例である第２角度制御モータ２１２の回転駆動力によって回転する。本実施形態では、第２プーリ２２２と第２角度制御モータ２１２の出力軸２１５とに第２ベルト２１６が巻き掛けられており、第２角度制御モータ２１２の出力軸２１５の回転がこの第２ベルト２１６を介して第２プーリ２２２に伝達されるように構成されている。
【００２０】
このような構成であると、姿勢変更部の例である第１角度制御モータ２１１の出力軸２１３の回転角度に応じて、第１湾曲部１１１の姿勢が変化する。同様に、第２角度制御モータ２１２の出力軸２１５の回転角度に応じて第２湾曲部１１２の姿勢が変化する。このように、第１湾曲部１１１と第２湾曲部１１２とはいずれも湾曲可能であり、それぞれ姿勢変更部の例である第１角度制御モータ２１１と第２角度制御モータ２１２の動作によって、互いに独立して円弧状に湾曲させることができる。なお、第１角度制御モータ２１１の回転動力を第１プーリ２２１に伝達するための駆動力伝達機構と、第２角度制御モータ２１２の回転動力を第２プーリ２２２に伝達するための駆動力伝達機構は、前記構成に限定されるものではない。要は、第１プーリ２２１の回転角度が第１角度制御モータ２１１の出力軸２１３の回転角度から一義的に定まる構成であり、第２プーリ２２２の回転角度が第２角度制御モータ２１２の出力軸２１５の回転角度から一義的に定まる構成であればよい。
【００２１】
なお、本実施形態では、連続体ロボット１が第１ワイヤ２３１と第２ワイヤ２３２を有する構成を例に示すが、これは説明の便宜上簡略化したものであり、これら以外のワイヤを有していてもよい。例えば、第２ワイヤ２３２のほかに、第１湾曲部１１１と第２湾曲部１１２に沿って設けられ、第１ワイヤ固定部２６１と第２ワイヤ固定部２６２の両方に固定されている固定ワイヤを有していてもよい。このような構成であれば、それぞれの湾曲部を３次元的に湾曲させることができる。また、図２に示す構成では、第１ワイヤ固定部２６１が第２ワイヤ固定部２６２よりも大径となっているが、この構成に限られない。たとえば、第１ワイヤ固定部２６１が第２ワイヤ固定部２６２とを同径として、第２ワイヤ固定部２６２に固定された第２ワイヤ２３２が、第１ワイヤ固定部に設けられた空孔を挿通するようにしてもよい。さらに、第１ワイヤ２３１は、それぞれ独立に駆動される複数のワイヤとして構成してもよい。第２ワイヤ２３２も同様である。
【００２２】
（連続体ロボットのモデル）
ここで、連続体ロボット１の先頭追従制御と駆動ユニット２の前後進動作の制御に用いるためのモデルについて説明する。本実施形態では、第１湾曲部１１１の姿勢の例である遠位端角度θ1と第１角度制御モータ２１１の出力軸２１３の回転角度の関係と、第２湾曲部１１２の姿勢の例である遠位端角度θ2と第２角度制御モータ２１２の出力軸２１５の回転角度との関係とを、連続体ロボット１のモデルを用いて規定する。そして、制御装置４は、モデルを用いて規定されたこれらの関係を用いて、連続体ロボット１の先頭追従制御と、駆動ユニット２の前後進動作の速度制御を行う。
【００２３】
モデル化に当たり、本実施形態で制御対象となる連続体ロボット１は、以下の（１）〜（４）の４つ条件を充足するものとする。
（１）  第１湾曲部１１１の曲率半径はその長尺方向の全長にわたって一定であり、第２湾曲部１１２の曲率半径もその長尺方向の全長にわたって一定である。
（２）  第１湾曲部１１１の中立面Ｎ1は第１湾曲部１１１の中心に位置しており、第２湾曲部１１２の中立面Ｎ2は第２湾曲部１１２の中心に位置している。
（３）  第１ワイヤ２３１と第２ワイヤ２３２は伸縮しない（伸縮を考慮しない）。
（４）  第１湾曲部１１１と第２湾曲部１１２の長尺寸法は等しい。
【００２４】
まず、第１湾曲部１１１のモデルについて説明する。図３は、第１湾曲部１１１のモデルを示す模式図である。図３に示すように、第１湾曲部１１１の近位端を原点Ｏ1とし、駆動ユニット２の前進動作の移動方向をＺ1軸とし、第１湾曲部１１１が姿勢変化（角度変化）する平面上においてＺ1軸と直角な軸をＸ1軸とする。第１湾曲部１１１の遠位端角度θ1は、第１湾曲部１１１の遠位端を通過し中心線（中立面Ｎ1）に直角な直線とＸ1軸とのなす角度である。φ1は、第１角度制御モータ２１１の出力軸２１３の回転角度を表す。ｒ1は、第１湾曲部１１１の中立面Ｎ1と第１ワイヤ２３１との距離を表す。前記条件（１）により、第１湾曲部１１１の中立面Ｎ1は円弧であり、円弧の中心角は第１湾曲部１１１の遠位端角度θ1と等しい。また、条件（２）より、中立面Ｎ1の長さをＬc、円弧の曲率中心と第１湾曲部１１１の中立面Ｎ1との距離をＲ1とすると、第１湾曲部１１１の遠位端角度θ1は、次の数式（１）で表される。
【００２５】
【数１】
2018175602.tif 000003
【００２６】
この場合、第１ワイヤ２３１のうち、第１湾曲部１１１の中立面Ｎ1の外側に位置する部分（説明の便宜上、外側部分２３１１と称する）の長さＬ1oは、前記条件（３）より、次の数式（２）で表される。
【００２７】
【数２】
2018175602.tif 000004
【００２８】
第１湾曲部１１１の遠位端角度θ1が０のとき、第１湾曲部１１１の中立面Ｎ1の長さＬcと第１ワイヤ２３１の長さＬ1oは一致する。このため、第１湾曲部１１１の遠位端角度を０からθ1まで変化させるために必要な第１ワイヤ２３１の外側部分２３１１の駆動量ΔＬ1oは、次の数式（３）で表される。
【００２９】
【数３】
2018175602.tif 000005
【００３０】
同様に、第１ワイヤ２３１のうちの第１湾曲部１１１の中立面Ｎ1の内側（曲率中心に近い側）に位置する部分（説明の便宜上、内側部分２３１２と称する）の長さＬ1iは、次の数式（４）で表される。
【００３１】
【数４】
2018175602.tif 000006
【００３２】
したがって、第１湾曲部１１１の遠位端角度を０からθ1まで変化させるために必要な第１ワイヤ２３１の内側部分２３１２の駆動量ΔＬ1iは、次の数式（５）で表される。
【００３３】
【数５】
2018175602.tif 000007
【００３４】
数式（３）と数式（５）より、第１ワイヤ２３１の外側部分２３１１と内側部分２３１２の駆動量は、大きさが等しく方向が反対である。第１角度制御モータ２１１の出力軸２１３の回転角度を０からφ1に変化させた場合、第１ワイヤ２３１の外側部分２３１１の駆動量ΔＬ1oと内側部分２３１２の駆動量ΔＬ1iは、次の数式（６）で表される。なお、ｒoは第１角度制御モータ２１１の出力軸２１３の半径である。
【００３５】
【数６】
2018175602.tif 000008
【００３６】
これらより、第１湾曲部１１１の遠位端角度θ1と第１角度制御モータ２１１の出力軸２１３の回転角度φ1の関係は、次の数式（７）で表される。
【００３７】
【数７】
2018175602.tif 000009
【００３８】
次に、第２湾曲部１１２の遠位端角度θ2と第２角度制御モータ２１２の出力軸２１５の回転角度φ2との関係について説明する。図４は、連続体ロボット１の第２湾曲部１１２のモデルを説明する模式図である。なお、図４では、第１角度制御モータ２１１と第１プーリ２２１を省略している。第２湾曲部１１２の遠位端角度θ2は、第２湾曲部１１２の遠位端を通過し中心線（中立面Ｎ2）に直角な直線とＸ1軸とのなす角度である。図４に示すように、第２湾曲部１１２の近位端を原点Ｏ2とし、第２湾曲部１１２の長尺方向をＺ2軸とし、第２湾曲部１１２が駆動する平面上においてＺ2軸に直角な軸をＸ2軸とする。なお、本実施形態では、第１湾曲部１１１と第２湾曲部１１２が同じ平面上で姿勢変化するため、Ｘ1−Ｚ1平面とＸ2−Ｚ2平面は一致している。
【００３９】
前記条件（２）と（３）によれば、第２湾曲部１１２の曲率半径Ｒ2を変更しても、第１湾曲部１１１の中立面Ｎ1および第１ワイヤ２３１の長さは変化しない。このため、第２湾曲部１１２の曲率半径Ｒ2を変更しても、第１湾曲部１１１について前記数式（７）が成立する。また、前記条件（１）より、第１湾曲部１１１と同様に、第２湾曲部１１２の中立面Ｎ2も円弧となる。前記条件（４）より、第２湾曲部１１２の長さは中立面Ｎ2の長さＬcと等しくなる。このため、Ｘ1軸に対する第２湾曲部１１２の遠位端角度をθ2とし、中立面Ｎ2と円弧の中心との距離をＲ2とすると、次の数式（８）が成立する。なお、数式（８）の左辺は、第２湾曲部１１２の円弧の中心角である。
【００４０】
【数８】
2018175602.tif 000010
【００４１】
第２ワイヤ２３２のうち、その中立面Ｎ2から曲率の半径方向外側に位置する部分（説明の便宜上、外側部分と称する）のうち、第２湾曲部１１２に沿う部分２３２１の長さＬ22oは、次の数式（９）で表される。ｒ2は、第２湾曲部１１２の中立面Ｎ2と第２ワイヤ２３２との距離である。
【００４２】
【数９】
2018175602.tif 000011
【００４３】
第２ワイヤ２３２のうち、第１湾曲部１１１の中立面Ｎ１の曲率の半径方向外側に位置する部分（説明の便宜上、外側部分と称する）のうち、第１湾曲部１１１に沿う部分２３２３の長さＬ21oは、数式（２）に示す第１湾曲部１１１と同様に、次の数式（１０）で表される。
【００４４】
【数１０】
2018175602.tif 000012
【００４５】
このため、第２ワイヤ２３２の外側部分２３２１，２３２３の和Ｌ2oは、数式（９）と数式（１０）とから、次の数式（１１）で表される。
【００４６】
【数１１】
2018175602.tif 000013
【００４７】
したがって、第２湾曲部１１２の遠位端角度がθ2のときの第２ワイヤ２３２の外側部分の駆動量ΔＬ2oは、次の数式（１２）で表される。
【００４８】
【数１２】
2018175602.tif 000014
【００４９】
同様に、第２ワイヤ２３２のうちの中立面Ｎ2より曲率の半径方向内側に位置する部分（以下、内側部分２３２２，２３２４と称する）の長さＬ22iとＬ21iは、それぞれ、次の数式（１３）と数式（１４）で表される。そして、第２ワイヤ２３２のうちの内側部分２３２２，２３２４の駆動量ΔＬ2iは、数式（１５）で表される。
【００５０】
【数１３】
2018175602.tif 000015
【００５１】
したがって、第２角度制御モータ２１２の出力軸２１５の回転角度を０からφ2に変化させた場合、出力軸２１５の回転角度φ2と第２湾曲部１１２の遠位端角度θ2との関係は、次の数式（１６）で表される。なお、ｒoは、第２角度制御モータ２１２の出力軸２１５の半径である。
【００５２】
【数１４】
2018175602.tif 000016
【００５３】
以上のとおり、連続体ロボット１のモデルから、第１角度制御モータ２１１の出力軸２１３の回転角度φ1と第１湾曲部１１１の遠位端角度θ1との関係が規定される。同様に、第２角度制御モータ２１２の出力軸２１５の回転角度φ2と第２湾曲部１１２の遠位端角度θ2との関係が規定される。そして、制御装置４は、これらの関係を用いて、連続体ロボット１の先頭追従制御と、駆動ユニット２の前後進動作の制御を行う。
【００５４】
なお、上記説明では、長尺方向に直列に連結するように設けられている２つの湾曲部を有する連続体ロボット１を示したが、湾曲部の数は限定されない。長尺方向に直列に連結するように設けられている３つ以上の湾曲部を有する連続体ロボットについても同様に、角度制御モータの出力軸の回転角度と遠位端湾曲部の遠位端角度との関係を規定できる。すなわち、前記条件（１）〜（４）によれば、第ｉ湾曲部の遠位端角度θiは、第ｉ湾曲部を駆動する第ｉ角度制御モータの出力軸の回転角度φiにのみ依存する。そして、第ｉ角度制御モータの出力軸の回転角度φiと第ｉ湾曲部の遠位端角度θiとの関係は、第ｉ湾曲部の中立面Ｎiと第ｉ湾曲部を駆動するワイヤとの距離ｒiを用いると、次の数式（１７）で表される。なお、ｉは正の整数であり、近位端湾曲部をｉ＝１とする。
【００５５】
【数１５】
2018175602.tif 000017
【００５６】
なお、本実施形態では、第１角度制御モータ２１１から第１湾曲部１１１の間と、第２角度制御モータ２１２から第２湾曲部１１２の間に減速機構が設けられない構成を例に示した。すなわち、第１角度制御モータ２１１の出力軸２１３の外周面の円周方向の移動量（回転角度×半径）と第１ワイヤ２３１の駆動量とが同じである。また、第２角度制御モータ２１２の出力軸２１５の外周面の円周方向の移動量（回転角度×半径）と第２ワイヤ２３２の駆動量とが同じである。ただし、このようなモデルに限定されない。例えば、第１角度制御モータ２１１から第１湾曲部１１１の間と、第２角度制御モータ２１２から第２湾曲部１１２の間に減速機構や倍力機構が設けられる構成であってもよい。この場合、数式（７）と数式（１６）と数式（１７）の右辺のそれぞれに、減速機構の減速比や倍力機構の倍力比に応じた係数を乗じればよい。
【００５７】
（制御装置）
次に、制御装置４について説明する。図５は、制御装置４の構成例を示すブロック図である。図５に示すように、制御装置４は、角度制御部４１と、角度記憶部４２と、移動駆動制御手段の例である駆動ユニット速度計算・制御部４４とを有する。角度記憶部４２は、駆動ユニット２から取得した駆動ユニット２の位置ｚと、角度入力部５から取得した第２湾曲部１１２の目標姿勢の例である遠位端の目標角度ψを記憶する。角度制御部４１は、第ｉ湾曲部（ｉ＝１，２）の位置がｓiとなるときの目標角度ψi（目標姿勢）を角度記憶部４２から読み出し、連続体ロボット１が先頭追従制御を行うように第１湾曲部１１１と第２湾曲部１１２のそれぞれの遠位端角度θ1，θ2を制御する。駆動ユニット速度計算・制御部４４は、駆動ユニット２の前後進動作の目標移動速度を計算する。そして、駆動ユニット速度計算・制御部４４は、駆動ユニット２の前後進動作の移動速度が計算した目標移動速度となるように移動駆動部３２を制御する。
【００５８】
制御装置４は、ＣＰＵとＲＯＭなどの記憶媒体とＲＡＭとを有するコンピュータを有している。ＲＯＭなどの記憶媒体には、駆動ユニット２を制御するためのコンピュータプログラムがあらかじめ格納されている。制御装置４のコンピュータのＣＰＵは、ＲＯＭなどの記憶媒体に格納されているこのコンピュータプログラムを読み出し、ＲＡＭをワークエリアとして用いて実行する。これにより、コンピュータが制御装置４の前記各部として機能し、後述する連続体ロボット１の制御と駆動ユニット２の速度制御が実現する。
【００５９】
（先頭追従制御）
ここで、制御装置４による連続体ロボット１の先頭追従制御について説明する。図６は、湾曲した部分を有する経路に沿って連続体ロボット１を前進動作させる際の先頭追従制御する例を示す。連続体ロボット１を挿入・抜去させる経路（以下、挿抜経路と称する）は連続体ロボット１が進行するのに十分な幅を有しているものとし、図６において、太い実線がこの挿抜経路を規定する境界を示している。また、図６における破線は、連続体ロボット１の目標経路を示しており、制御装置４は連続体ロボット１の各湾曲部がこの目標経路Ｔに沿って挿入・抜去されるように（前後進動作するように）制御する。なお、目標経路Ｔは、挿抜経路の形状などに応じて使用者等が適宜決定するものである。例えば、軟性内視鏡であれば、使用者がカメラ６により撮影された画像に基づいて目標経路Ｔを設定したり、ＭＲＩ検査により挿抜経路を把握して目標経路Ｔを設定したりする。
【００６０】
図６に示すように、挿抜経路は曲がっている部分を有しており、このため目標経路Ｔも曲がっている部分を有している。制御装置４は、挿抜経路の曲がっている部分においても連続体ロボット１の第１湾曲部１１１と第２湾曲部１１２が目標経路Ｔから逸脱しないように、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2を制御する。換言すると、連続体ロボット１の複数の湾曲部が挿抜経路の外部に逸脱しないように、遠位端角度θ1，θ2を制御する。目標経路Ｔに沿って座標軸ｓをとると、連続体ロボット１の各湾曲部が目標経路Ｔ上にあるとき、駆動ユニット２のｚ方向の移動量と各湾曲部のｓ方向の移動量が一致する。そこで、本実施形態では、位置ｓを用いて各湾曲部の前後進方向の位置を表す。
【００６１】
図６（ａ）は、連続体ロボット１の初期状態の例を示す。図６（ａ）に示すように、初期状態における第２湾曲部１１２の遠位端角度θ2をψnとし、位置ｓ2をｎＬcとする。また、初期状態における第１湾曲部１１１の遠位端角度θ1をψn-1とし、位置ｓ1を（ｎ−１）Ｌcとする。なお、ｎは正の整数であり、Ｌcは第１湾曲部１１１および第２湾曲部１１２のそれぞれの長尺方向寸法（中立面Ｎ1，Ｎ2の長尺方向寸法）である。
【００６２】
連続体ロボット１を前進動作させる場合には、使用者は角度入力部５を用いて、第２湾曲部１１２が位置（ｎ＋１）Ｌcに到達するときの第２湾曲部１１２の目標姿勢の例である遠位端の目標角度をψn+1を入力する。角度記憶部４２は、各湾曲部の現在の位置（ｎ−１）Ｌc、ｎＬcと目標位置（ｎ＋１）Ｌc、遠位端の現在の角度ψn-1、ψnと遠位端の目標角度ψn+1をそれぞれ記憶する。角度制御部４１は、角度記憶部４２に記憶された角度ψn-1、ψn、ψn+1を読み込む。そして、角度制御部４１は、次に示す数式（１８）と数式（１９）を用いて、連続体ロボット１が初期位置から距離ｌ  （０≦ｌ≦Ｌc）  移動したときの第１湾曲部１１１と第２湾曲部１１２のそれぞれの目標姿勢の例である遠位端角度θ1，θ2を計算する。
【００６３】
【数１６】
2018175602.tif 000018
【００６４】
図６（ｂ）は、連続体ロボット１が図６（ａ）に示す初期状態から距離Ｌc（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）進んだ状態を示す。図６（ｂ）に示すように、移動駆動部３２によって駆動ユニット２が前進すると、それに伴って連続体ロボット１が距離Ｌc前進する。そしてこの場合、角度制御部４１は、数式（７）と数式（１６）を用いて、遠位端角度θ1，θ2から、第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の目標回転角度φ1，φ2を計算する。そして、角度制御部４１は、計算した目標回転角度φ1，φ2となるように、第１角度制御モータ２１１と第２角度制御モータ２１２を制御する。数式（１８）と数式（１９）より明らかなように、駆動ユニット２がｌ＝０からｌ＝Ｌcへ前進すると、第１湾曲部１１１の遠位端角度θ1はψn-1からψnへ、第２湾曲部１１２の遠位端角度θ2はψnからψn+1へ、それぞれ連続的に変化する。従って、図６（ｃ）に示すようｓ1＝ｎＬcにおける第１湾曲部１１１の姿勢は、同じ位置における第２湾曲部１１２の姿勢と一致する。
【００６５】
連続体ロボット１が距離Ｌc（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）前進するごとに、角度制御部４１は同様の処理を実行する。すなわち、目標位置（ｎ＋２）Ｌcにおける第２湾曲部１１２の目標角度をψn+2を入力して連続体ロボット１を前進させると、角度制御部４１は、次の数式（２０）と数式（２１）を用いて遠位端角度θ1，θ2を計算する。そして、角度制御部４１は、これらと各湾曲部の遠位端角度が目標角度と一致するように第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度を制御する。なお、数式（２０）と数式（２１）は、それぞれ数式（１８）と数式（１９）中のｎをｎ＋１と置き換えたものである。
【００６６】
【数１７】
2018175602.tif 000019
【００６７】
数式（１８）と数式（２１）が同じであることから明らかなように、第１湾曲部１１１は、第２湾曲部１１２がｓ2＝ｎＬcからｓ2＝（ｎ＋１）Ｌcへ前進するときに通過した経路を追従するように通過する。このように、先頭追従制御が実現する。
【００６８】
連続体ロボット１を後進動作させる際の制御は次のとおりである。角度制御部４１は前進動作時に通過した経路に沿うように、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2を制御する。説明の便宜上、後進動作を開始するときの第２湾曲部１１２の遠位端角度θ2をψm+1とし、位置ｓ2を（ｍ＋１）Ｌcとする。同様に、第１湾曲部１１１の遠位端角度θ1をψmとし、位置ｓ1をｍＬcとする。なお、ｍはｎ＜ｍを満たす正の整数を表す。
【００６９】
前述のように、角度記憶部４２は、前進動作時に入力された目標角度ψm-1、ψm、ψm+1を記憶している。そこで、角度制御部４１は、まず、角度記憶部４２から目標角度ψm-1、ψm、ψm+1を読み込む。そして、次に示す数式（２２）と数式（２３）を用いて遠位端角度θ1，θ2を計算し、これらと一致するように第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度を制御する。
【００７０】
【数１８】
2018175602.tif 000020
【００７１】
そして、前進動作時と同様に、連続体ロボット１が距離Ｌc（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）後進するごとに同様の制御を繰り返す。これにより、第１湾曲部１１１の通過した経路に沿って第２湾曲部１１２が後進動作を行う。このように、後進動作時においても、先頭追従制御が実行される。
【００７２】
（速度制御）
次いで、連続体ロボット１の前後進動作の速度制御について説明する。連続体ロボット１の挿入・抜去を短時間で行うには、駆動ユニット２をできるだけ高速に前後進させることが望ましい。しかしながら、第１角度制御モータ２１１と第２角度制御モータ２１２の回転速度には物理的な上限が存在する。このため、第１角度制御モータ２１１と第２角度制御モータ２１２が動作開始してから第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2が目標角度に一致するまでには、ある程度の時間を要する。したがって、連続体ロボット１を高速で前後進（すなわち、挿入・抜去）すると、第１湾曲部１１１や第２湾曲部１１２の遠位端角度θ1，θ2が目標角度に一致するよりも早く、連続体ロボット１が距離Ｌcの進行が完了することがある。そうすると、第１湾曲部１１１や第２湾曲部１１２が目標経路Ｔから逸脱して挿抜経路と外部との境界に接触するおそれがある。例えば、本実施形態に係る連続体ロボットシステムが軟性内視鏡に適用される場合には、連続体ロボット１が被検者の体組織の内面に接触するおそれがある。
【００７３】
そこで、制御装置４は、ボリューム７２により入力（設定）された要求移動速度ｖrefで駆動ユニット２を前進動作または後進動作させた場合に、距離Ｌcの進行が完了する以前に第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2を目標角度に一致させることができるか否かを判断する。すなわち、要求移動速度ｖrefで駆動ユニット２を前後進動作させた場合に、連続体ロボット１が目標経路Ｔから逸脱することなく距離Ｌc進行させることができるか否かを判断する。
【００７４】
本実施形態では、駆動ユニット速度計算・制御部４４は、連続体ロボット１の各湾曲部が目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値を計算する。本実施形態では、「湾曲部が目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値」を、「追従可能速度」と称する。そして、駆動ユニット速度計算・制御部４４は、設定された要求移動速度ｖrefが計算した追従可能速度以下（目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値以下）であるか否かを判断する。設定された要求移動速度ｖrefが計算した追従可能速度以下である場合には、連続体ロボット１を要求移動速度ｖrefで前後進させても連続体ロボット１が目標経路Ｔから逸脱しないようにできることを意味している。そこでこの場合には、駆動ユニット速度計算・制御部４４は、連続体ロボット１を要求移動速度ｖrefで前後進動作させても目標経路Ｔから逸脱することなく追従制御が可能であると判断する。そして、移動駆動部３２による駆動ユニット２（連続体ロボット１）の目標移動速度を要求移動速度ｖrefに設定する。
【００７５】
一方、設定された要求移動速度ｖrefが計算した追従可能速度を超える場合には、連続体ロボット１を要求移動速度ｖrefで前後進させると、連続体ロボット１が目標経路Ｔから逸脱するおそれがあることを意味している。そこでこの場合には、駆動ユニット速度計算・制御部４４は、連続体ロボット１を要求移動速度ｖrefで前後進動作させると連続体ロボット１が目標経路Ｔから逸脱するおそれがあると判断し、目標移動速度を計算した追従可能速度以下の移動速度に設定する。そして、駆動ユニット速度計算・制御部４４は、移動駆動部３２を制御し、このように設定した目標移動速度で駆動ユニット２を前後進させる。このような構成によれば、連続体ロボット１が目標経路Ｔから逸脱することを防止できる。
【００７６】
なお、連続体ロボット１の前後進の移動速度を低くし過ぎた場合であっても、連続体ロボット１が目標経路Ｔから逸脱するおそれがある。また、連続体ロボット１の前後進速度を低くすると、連続体ロボット１が目標経路Ｔから逸脱しないまでも、挿入・抜去に要する時間が長くなる。そこで、駆動ユニット速度計算・制御部４４は、目標移動速度の下限の閾値を設定し、設定した閾値以上の移動速度で駆動ユニット２が前後進するように移動駆動部３２を制御してもよい。この閾値は追従可能速度以下であればよく、適宜設定される。ただし、挿入・抜去時間の短縮の観点からは、この閾値はできるだけ高い値であることが好ましい。
【００７７】
具体的な制御の内容は次のとおりである。まず、連続体ロボット１が目標経路Ｔから逸脱することなく前後進動作できる移動速度、すなわち、連続体ロボット１が目標経路Ｔから逸脱することなく前後進動作できる移動速度の条件について説明する。数式（７）の両辺を時間で微分すると、次の数式（２４）が得られる。
【００７８】
【数１９】
2018175602.tif 000021
【００７９】
また、式（１９）を時間で微分すると、駆動ユニット２（連続体ロボット１）の前進動作中はψnとψn-1が定数であることから、次の数式（２５）が得られる。
【００８０】
【数２０】
2018175602.tif 000022
【００８１】
数式（２４）と数式（２５）とから、次の数式（２６）が得られる。
【００８２】
【数２１】
2018175602.tif 000023
【００８３】
数式（２６）より、距離Ｌc移動する間に第１湾曲部１１１の遠位端角度θ1を目標角度に一致させることができる移動速度ｖの最大値、すなわち、第１湾曲部１１１についての追従可能速度ｖu1は、次の数式（２７）で表される。なお、ωmaxは、第１角度制御モータ２１１の出力軸２１３の出力可能な最大角速度である。
【００８４】
【数２２】
2018175602.tif 000024
【００８５】
連続体ロボット１の移動速度ｖが数式（２７）で計算される第１湾曲部１１１についての追従可能速度ｖu1以下である場合には、距離Ｌc移動する間に（移動が完了するまでに）、第１湾曲部１１１の遠位端角度θ1を目標角度に一致させることができる。
【００８６】
同様に、数式（１６）と数式（１８）より、第２湾曲部１１２についての追従可能速度ｖu2は、次の数式（２８）で表される。連続体ロボット１の移動速度ｖが数式（２８）で計算される追従可能速度ｖu2以下である場合には、距離Ｌc移動する間に（移動が完了するまでに）、第２湾曲部１１２の遠位端角度θ2を目標角度に一致させることができる。なお、ωmaxは、第２角度制御モータ２１２の出力軸２１５の出力可能な最大角速度である。
【００８７】
【数２３】
2018175602.tif 000025
【００８８】
なお、数式（７）と数式（２３）、数式（１６）と数式（２２）を用いて同様の計算を行うと、連続体ロボット１（駆動ユニット２）が後進する際における第１湾曲部１１１と第２湾曲部１１２の追従可能速度ｖu1，ｖu2も、数式（２７）と数式（２８）で与えられることが明らかである。
【００８９】
したがって、連続体ロボット１を距離Ｌc移動させる間に第１湾曲部１１１と第２湾曲部１１２の両方が目標経路Ｔから逸脱しないようにするためには、連続体ロボット１の移動速度ｖが、第１湾曲部１１１と第２湾曲部１１２についての追従可能速度のうちの小さい方の値以下であればよい。ただし、数式（２７）と数式（２８）は、モデルを用いて導出した計算式であり、誤差が存在している場合がある。そして、連続体ロボット１のモデルに誤差が存在すると、連続体ロボット１が目標経路Ｔから逸脱するおそれがある。そこで、本実施形態では、モデルの誤差を考慮して、数式（２７）と数式（２８）で計算される追従可能速度ｖu1，ｖu2のうちの小さい方に係数αを乗じた値を計算する。この係数αの値は、０より大きく１以下である。なお、この係数αの具体的な値は、モデルに含まれる誤差の大きさに応じて適宜設定すればよい。例えば、係数αの値は、誤差が大きいほど小さく、誤差が小さいほど大きいことが好ましい。また、誤差が無視できる程度であれば、係数αを乗じなくてもよい（係数αの値を１にしてもよい）。
【００９０】
そして、駆動ユニット速度計算・制御部４４は、この値と連続体ロボット１（駆動ユニット２）の要求移動速度ｖrefが数式（２９）に示す条件を充足するか否かを判断する。すなわち、要求移動速度ｖrefが、それぞれの湾曲部についての追従可能速度のうちの最も小さい値に係数αを乗じた値以下（モデルの誤差を考慮した追従可能速度以下）であるか否かを判断する。
【００９１】
【数２４】
2018175602.tif 000026
【００９２】
駆動ユニット速度計算・制御部４４は、数式（２９）が成立する場合には、連続体ロボット１の目標移動速度（すなわち、移動駆動部３２による駆動ユニット２の目標移動速度）を要求移動速度ｖrefに設定する。一方、要求移動速度ｖrefにつて数式（２９）が成立しない場合には、駆動ユニット２の目標移動速度を数式（２９）の右辺で計算される値に設定する。数式（２９）の右辺で計算される値は、それぞれの湾曲部についての追従可能速度のうちの最も小さい値に係数αを乗じた値（すなわち、モデルの誤差を考慮した追従可能速度）である。このように、駆動ユニット速度計算・制御部４４は、駆動ユニット２の目標移動速度を、追従可能速度以下に制限する（移動速度制限手段）。
【００９３】
駆動ユニット速度計算・制御部４４は、駆動ユニット２が距離Ｌc前後進動作するごとに、上記計算および制御を繰り返す。このような速度制御によれば、連続体ロボット１が目標経路Ｔから大きく逸脱しないようにできる。また、連続体ロボット１を、目標経路Ｔに追従可能な移動速度の最大速度で移動させることができる。このため、連続体ロボット１の挿抜経路への挿入・抜去に要する時間が長くなることを抑制できる。
【００９４】
（処理の例）
次に、制御装置４が実行する処理の例について説明する。図７は、制御装置４が実行する処理の例を示すフローチャートである。なお、この処理を実行するためのコンピュータプログラムは、あらかじめ制御装置４のコンピュータのＲＯＭなどの記憶媒体に格納されている。そして、コンピュータのＣＰＵはこのコンピュータプログラムをＲＯＭなどの記憶媒体から読み出し、ＲＡＭをワークエリアとして用いて実行する。これにより、コンピュータが制御装置４の前記各部として機能し、この処理が実現する。
【００９５】
ステップＳ１０１の「要求移動速度・目標角度入力有？」において、駆動ユニット速度計算・制御部４４は、ガイド操作部７に対して要求移動速度ｖrefの入力の操作が有り、かつ、角度入力部５に対して目標角度の入力が有るかを判断する。これらのいずれか一方でも入力がない場合には、このステップＳ１０１で待機する。両方の入力が有った場合にはステップＳ１０２に進む。
【００９６】
ステップＳ１０２の「位置、要求移動速度、角度記憶」において、制御装置４の角度記憶部４２は、角度入力部５から目標角度を取得して記憶する。また、駆動ユニット２の位置・速度計算部から駆動ユニット２の位置を取得して記憶し、ガイド操作部７から要求移動速度ｖrefを取得して記憶する。
【００９７】
ステップＳ１０３の「距離Ｌc移動？」において、駆動ユニット速度計算・制御部４４は、ステップＳ１０１でガイド操作部７に対して要求移動速度ｖrefの入力の操作が有り、かつ、角度入力部５に対して目標角度の入力の操作が有ってから連続体ロボット１が距離Ｌc移動したか否かを判断する。移動した場合にはステップＳ１０１に戻る。移動していない場合にはステップＳ１０４に進む。
【００９８】
ステップＳ１０４の「追従可能速度計算／要求移動速度読み出し」において、駆動ユニット速度計算・制御部４４は、角度記憶部４２に記憶されている第２湾曲部１１２の遠位端の目標角度θ2を読み出す。そして、数式（１８）、数式（１９）、数式（２２）、数式（２３）を用いて第１湾曲部１１１と第２湾曲部１１２の目標角度を計算し、計算した目標角度を用いて追従可能速度を計算する。また、駆動ユニット速度計算・制御部４４は、角度記憶部４２に記憶されている要求移動速度ｖrefを読み出す。
【００９９】
ステップＳ１０５の「追従条件充足？」において、駆動ユニット速度計算・制御部４４は、ガイド操作部７から取得した要求移動速度ｖrefが、数式（２９）の条件を充足するかどうかを判断する。すなわち、駆動ユニット速度計算・制御部４４は、要求移動速度ｖrefが、計算したそれぞれの湾曲部についての追従可能速度のうちで最も小さい追従可能速度に係数αを乗じた値以下であるか否かを判断する。これにより、駆動ユニット速度計算・制御部４４は、駆動ユニット２を要求移動速度ｖrefで前後進動作させた場合に、連続体ロボット１が目標経路Ｔから逸脱するか否かを判断する。数式（２９）の条件を充足する場合（Ｙｅｓ）にはステップＳ１０６に進み、充足しない場合（Ｎｏ）にはステップＳ１０７に進む。
【０１００】
ステップＳ１０６の「目標移動速度＝要求移動速度」において、駆動ユニット速度計算・制御部４４は、移動駆動部３３による駆動ユニット２の目標移動速度を、ガイド操作部７から取得した要求移動速度ｖrefに設定する。一方、ステップＳ１０７の「目標移動速度＝追従可能速度」において、駆動ユニット速度計算・制御部４４は、駆動ユニット２の目標移動速度を、それぞれの湾曲部についての追従可能速度のうちで最も小さい追従可能速度に係数αを乗じた値に設定する。
【０１０１】
ステップＳ１０８の「移動指令有？」においては、駆動ユニット速度計算・制御部４４は、前進動作の指令または後進動作の指令があるか否かを判断する。前進動作の指令と後進動作の指令のいずれもない場合にはステップＳ１０５に戻る。前進動作の指令または後進動作の指令がある場合には、ステップＳ１０９に進む。
【０１０２】
ステップＳ１０９の「湾曲部駆動、駆動ユニット移動」において、角度制御部４１は、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2が計算した目標角度に一致するように、第１角度制御モータ２１１と第２角度制御モータ２１２を駆動する。すなわち、数式（７）と数式（１６）で規定される回転角度だけ、第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５を回転させる。また、駆動ユニット速度計算・制御部４４は、移動駆動部３２を制御し、ステップＳ１０６またはステップＳ１０７において設定した目標移動速度で駆動ユニット２を前進動作または後進動作させる。これにより、連続体ロボット１を、設定した目標移動速度で距離Ｌc移動させる。
【０１０３】
ステップＳ１１０の「距離Ｌc移動？」において、駆動ユニット速度計算・制御部４４は、連続体ロボット１の移動距離が距離Ｌcに達したか否かを判断する。連続体ロボット１の移動距離が距離Ｌcに達していない場合（Ｎｏ）には、ステップＳ１０９に戻り移動を継続する。連続体ロボット１の移動距離が距離Ｌcに達した場合（Ｙｅｓ）には、ステップＳ１１１に進む。
【０１０４】
ステップＳ１１１の「移動停止」において、駆動ユニット速度計算・制御部４４は、移動駆動部３２の駆動を停止し、駆動ユニット２の前後進動作を停止させて、連続体ロボット１の前後進動作を停止させる。そして、ステップＳ１０１に戻る。
【０１０５】
このような処理によれば、要求移動速度ｖrefで駆動ユニット２を前後進させても連続体ロボット１が目標経路Ｔから逸脱しない場合には、要求移動速度ｖrefを目標移動速度として連続体ロボット１を前後進動作させる。一方、要求移動速度ｖrefでは連続体ロボット１が目標経路Ｔから逸脱するおそれがある場合には、追従可能速度を目標移動速度として駆動ユニット２を前後進動作させる。この場合の目標移動速度は、要求移動速度ｖrefよりも低い速度である。このように、このように、駆動ユニット速度計算・制御部４４は、目標移動速度の上限値を、追従可能速度以下の値に制限する移動速度制限手段として機能する。このような構成によれば、連続体ロボット１を前進や後進させる際に、連続体ロボット１が目標経路Ｔから大きく逸脱しないようにできる。
【０１０６】
さらに、本発明の実施形態に係る連続体ロボットシステムによれば、連続体ロボット１を目標経路Ｔから逸脱することなく、高速に挿入・抜去することができる。すなわち、追従可能速度は、連続体ロボット１が目標経路Ｔから逸脱しないようにできる最大速度であるから、連続体ロボット１の目標移動速度を追従可能速度に設定することにより、連続体ロボット１の挿入・抜去の速度の低下を最小限に抑制できる。
【０１０７】
そして、本発明の実施形態に係る連続体ロボットシステムが適用された軟性内視鏡によれば、挿抜経路（目標経路Ｔ）が複雑に湾曲している場合であっても、被検者の体組織に接触することなく、軟性内視鏡の遠位端（すなわち、連続体ロボット１の遠位端）を被検者の体内の深部に到達させることができる。したがって、被検者に与える負担を低減することができる。さらに、本発明の実施形態に係る連続体ロボットシステムが適用された軟性内視鏡によれば、軟性内視鏡を被検者の体組織に接触させることなく高速で挿入・抜去することができる。前記のとおり、追従可能速度は、連続体ロボット１が目標経路Ｔから逸脱しないようにできる最大速度であるから、連続体ロボット１の目標移動速度を追従可能速度に設定することにより、連続体ロボット１の挿入・抜去の速度の低下を最小限に抑制できる。このため、軟性内視鏡を被検者の体内に挿入している時間を短縮でき、被検者に与える負担を低減することができる。また、使用者（検査者）の作業時間の短縮を図ることができ、使用者（検査者）に係る負担も低減できる。
【０１０８】
＜第２の実施形態＞
次いで、第２の実施形態について説明する。なお、第１の実施形態と共通する構成には同じ符号を付し、説明を省略する。第２の実施形態は、移動駆動部３２が連続体ロボット１を駆動ユニット２とともに前後進動作させる形態ではなく、使用者が手動で前後進させる形態である。第１実施形態と同様に、連続体ロボット１は、目標経路Ｔから逸脱しない条件を充足する範囲において、できるだけ高速に前後進させることが好ましい。しかしながら、使用者が手動で連続体ロボット１（駆動ユニット２）を前後進させる構成では、前後進動作の移動速度を高精度に制御することは困難である。そこで、本実施形態では、連続体ロボット１の前後進動作の移動速度が高くなり、連続体ロボット１が目標経路Ｔから逸脱しない条件を充足しなくなった場合には、駆動ユニット２を制動して減速させる。これにより、使用者が連続体ロボット１（駆動ユニット２）を前後進させる際に、連続体ロボット１が目標経路Ｔから逸脱しないようにする。
【０１０９】
（連続体ロボットシステム）
図８は、第２の実施形態に係る連続体ロボットシステムの構成例を模式的に示す図である。図８に示すように、本実施形態に係る連続体ロボットシステムは、連続体ロボット１と、駆動ユニット２と、リニアガイド３と、制御装置４と、角度入力部５と、カメラ６と、ガイド操作部７とを含んでいる。第２の実施形態に係る連続体ロボットシステムは、第１の実施形態と比較すると、移動駆動部３２を有しておらず、ブレーキ部３４を有している。ブレーキ部３４は、後述する制御装置４のブレーキ制御部３４による制御により、駆動ユニット２を制動して前後進動作速度を低下させる。なお、ブレーキ部３４の構成は特に限定されるものではなく、例えば電磁ブレーキなど、公知の各種ブレーキ機構が適用できる。
【０１１０】
（制御装置）
図９は、本実施形態に係る制御装置４の構成例を示すブロック図である。本実施形態に係る制御装置４は、第１の実施形態に係る制御装置４と比較すると、駆動ユニット速度計算・制御部４４を有しておらず、代わりにブレーキ部３４を制御するブレーキ制御部３４を有している。それ以外は共通の構成が適用できる。
【０１１１】
前述のとおり、第１角度制御モータ２１１と第２角度制御モータ２１２の最大回転数は有限であるため、駆動ユニット２の前後進動作の速度が高くなると、連続体ロボット１が目標経路Ｔから逸脱するおそれがある。そこで、ブレーキ制御部３４は、駆動ユニット２の移動速度ｖと、第ｉ湾曲部の位置がｓiのときの目標角度ψiから、連続体ロボット１が目標経路Ｔから逸脱しないようにできる第１角度制御モータ２１１と第２角度制御モータ２１２の回転速度を計算する。そして、ブレーキ制御部３４は、計算した回転速度が、第１角度制御モータ２１１と第２角度制御モータ２１２の最大回転速度を超える場合には、ブレーキ部３４を制御して駆動ユニット２を減速させる。これにより、連続体ロボット１（駆動ユニット２）の速度が、連続体ロボット１が目標経路Ｔから逸脱しないという条件を充足する範囲を超えないようにする。
【０１１２】
（ブレーキ制御部の処理の例）
ここで、ブレーキ制御部３４の処理の例について説明する。第１の実施形態と同様に、連続体ロボット１が所定の距離Ｌc前進動作する際の第１湾曲部１１１と第２湾曲部１１２の追従可能速度ｖu1，ｖu2は、第１の実施形態と同様に、それぞれ数式（２７）と数式（２８）で表される。また、数式（７）と数式（２３）、数式（１６）と数式（２２）を用いて同様の計算を行うことで、連続体ロボット１が後進動作する際の第１湾曲部１１１と第２湾曲部１１２の追従可能速度ｖu1，ｖu2も、数式（２７）と数式（２８）で与えられることが明らかである。
【０１１３】
そこで、ブレーキ制御部３４は、移動速度ｖが、数式（２７）と数式（２８）で計算される追従可能速度ｖu1，ｖu2以下であるか否かを判断する。そして、移動速度ｖが少なくとも一方の追従可能速度ｖu1，ｖu2より大きい場合には、ブレーキ部３４を制御して駆動ユニット２を制動して減速させる。また、ブレーキ制御部３４は、移動速度ｖが数式（２７）と数式（２８）で計算される両方の追従可能速度ｖu1，ｖu2以下である場合には、ブレーキ部３４を作動させず、ブレーキ部３４が作動中であれば作動を解除する。なお、第１の実施形態と同様に、モデルの誤差を考慮して値が１以下の係数αを用い、移動速度ｖが数式（２９）を充足するか否かを判断してもよい。
【０１１４】
具体的には、ブレーキ制御部３４は、連続体ロボット１の位置ｓを取得し、連続体ロボット１（駆動ユニット２）が距離Ｌc移動するごとに角度記憶部４２から目標角度を読み込み、数式（２７）と数式（２８）を用いて追従可能速度ｖu1，ｖu2を計算する（速度計算手段）。なお、連続体ロボット１の位置ｓは、駆動ユニット２の位置ｚを用いてブレーキ制御部３４が計算してもよく、角度制御部４１が計算した駆動ユニット２の位置ｚをブレーキ制御部３４が取得してもよい。そして、ブレーキ制御部３４は、移動速度ｖが追従可能速度ｖu1，ｖu2以下であるか否かの判断と、判断結果に基づくブレーキ部３４の制御（ブレーキ部３４の作動や解除）を、所定の制御周期で繰り返し実行する。この際、ある１つの制御周期においては、次の制御周期における移動速度ｖを推定し、推定した移動速度（以下、推定移動速度ｖest）が両方の追従可能速度ｖu1，ｖu2以下であるか否かを判断する。
【０１１５】
推定移動速度ｖestは、例えば、前記ある１つの制御周期における移動速度ｖの値をそのまま用いる構成が適用できる。この場合には、前記ある１つの制御周期における移動速度ｖと次の制御周期における推定移動速度ｖestとが同じであると見做せるように、制御周期を小さくすればよい。また、ある１つの制御周期における移動速度と加速度から、次の制御周期における推定移動速度ｖestを計算してもよい。この場合、  次の制御周期の推定移動速度ｖest  は、  （次の制御周期の推定移動速度ｖest）  ＝  （今回の制御周期の移動速度ｖ）+（取得した加速度）×（制御周期）  で計算できる。また、ブレーキ制御部３４は、この計算に用いる駆動ユニット２の加速度を、駆動ユニット２から取得する。この場合、駆動ユニット２に加速度センサが設けられ、この加速度センサによる加速度の検出結果を取得する構成が適用できる。
【０１１６】
このような構成によれば、第１角度制御モータ２１１の出力軸２１３の角速度と、第２角度制御モータ２１２の出力軸２１５の角速度とを、常に最大角速度以下に維持できる。すなわち、本実施形態では、ブレーキ制御部４３が、駆動ユニット２の移動速度ｖを追従可能速度以下に制限する（移動速度制限手段）。特に、推定移動速度ｖestを用いる構成によれば、駆動ユニット２の制動の遅れを防止できる。したがって、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2を目標角度に一致させ、第１湾曲部１１１と第２湾曲部１１２が目標経路Ｔから大きく逸脱することを抑制できる。また、駆動ユニット２の減速を、連続体ロボット１が目標経路Ｔから逸脱しないようにするための最小限に抑制できる。
【０１１７】
（制御装置の処理）
次に、制御装置４が実行する処理の例について説明する。図１０と図１１は、制御装置４が実行する処理の例を示すフローチャートである。なお、図１０は処理全体を示し、図１１は図１０のＡ〜Ｂの間の処理を示す。
【０１１８】
ステップＳ２０１の「目標角度入力有？」において、角度制御部４１は、目標角度の入力が有ったか否かを判断する。目標角度の入力がない場合（Ｎｏ）にはこのステップ２０１で待機する。目標角度の入力が有った場合（Ｙｅｓ）にはステップＳ２０２に進む。
【０１１９】
ステップＳ２０２の「位置、目標角度記憶」において、角度記憶部４２は、駆動ユニット２の位置・速度計算部から連続体ロボット１の位置を取得して記憶し、角度入力部５から入力された目標角度を取得して記憶する。そしてステップＳ２０３に進む。
【０１２０】
ステップＳ２０３の「距離Ｌc移動？」において、ブレーキ制御部３４は、ステップＳ２０１で角度入力部５に対して目標角度の入力が有ってから駆動ユニット２が距離Ｌc移動したか否かを判断する。移動した場合にはステップＳ２０１に戻る。移動していない場合にはステップＳ２０４とステップＳ２０６に進む。
【０１２１】
ステップＳ２０４の「目標角度読み込み」において、角度制御部４１は、角度記憶部４２に記憶されている目標角度を読み出す。そしてステップＳ２０５に進む。
【０１２２】
ステップＳ２０５の「湾曲部駆動」において、角度制御部４１は、数式（１８）と数式（１９）を用いて第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2を計算する。さらに、角度制御部４１は、数式（１６）と数式（１７）を用いて第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度φ1，φ2を計算する。そして、角度制御部４１は、計算した回転角度φ1，φ2となるように、第１角度制御モータ２１１と第２角度制御モータ２１２を制御する。これにより、第１湾曲部１１１と第２湾曲部１１２が駆動され、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ1，θ2が目標角度に一致する。
【０１２３】
ステップＳ２０６の「目標角度読み込み」において、ブレーキ制御部３４は、角度記憶部４２に記憶されている目標角度を読み出す。そしてステップＳ２０７に進む。
【０１２４】
ステップＳ２０７の「追従可能速度計算」において、ブレーキ制御部３４は、読み出した目標角度を用いて、第１湾曲部１１１と第２湾曲部１１２のそれぞれについての追従可能速度ｖu1，ｖu2を計算する。そしてステップＳ２０８に進む。
【０１２５】
ステップＳ２０８の「移動速度取得」において、ブレーキ制御部３４は、駆動ユニット２の位置・速度計算部から、連続体ロボット１（駆動ユニット２）の現在の移動速度を取得する。取得した現在の移動速度が、今制御周期における連続体ロボット１の移動速度である。そしてステップＳ２０９に進む。
【０１２６】
ステップＳ２０９の「追従条件充足？」において、ブレーキ制御部３４は、ステップＳ２０９において取得した今制御周期の連続体ロボット１の移動速度から、次の制御周期の連続体ロボット１の推定移動速度ｖestを計算する。そして、ブレーキ制御部３４は、推定移動速度ｖestが数式（２７）と数式（２８）で計算される追従可能速度ｖu1，ｖu2の両方以下である否かを判断する。少なくとも一方より大きい場合（Ｎｏ）にはステップＳ２１０に進み、両方以下である場合（Ｙｅｓ）にはステップＳ２１１に進む。なお、数式（２９）の左辺を推定移動速度ｖestに置き換えた数式が成立するか否かを判断してもよい。この場合、成立しなければ（Ｎｏ）にはステップＳ２１０に進み、成立すれば（Ｙｅｓ）にはステップＳ２１１に進む。
【０１２７】
ステップＳ２１０の「ブレーキ作動」において、ブレーキ制御部３４は、ブレーキ部３４を作動させ、駆動ユニット２を制動して連続体ロボット１を減速させる。そしてステップＳ２１２に進む。なお、現在すでにブレーキ部３４が作動している場合には、ブレーキ部３４の作動を継続する。
【０１２８】
ステップＳ２１１の「ブレーキ解除」において、ブレーキ制御部３４は、ブレーキ部３４の作動を解除する。そしてステップＳ２１２に進む。なお、ブレーキ部３４が作動していない場合には、ブレーキ部３４が作動していない状態を継続する。
【０１２９】
ステップＳ２１２の「制御周期経過？」においては、ブレーキ制御部３４は、１制御周期が経過したか否かを終了したかを判断する。１制御周期が経過していない場合には、今制御周期が終了していないとして、ステップＳ２１０またはステップＳ２１１の処理を継続する。１制御周期が経過した場合には、ステップＳ２１３に進む。
【０１３０】
ステップＳ２１３の「距離Ｌc移動？」において、ブレーキ制御部３４は、ステップＳ２０１で角度入力部５に対して目標角度の入力が有ってから連続体ロボット１が距離Ｌc移動したか否かを判断する。移動していない場合にはステップＳ２２０８に戻り、次の制御周期を開始する。移動した場合にはステップＳ２０１に戻る（図１０参照）。
【０１３１】
このように、使用者による連続体ロボット１の移動速度が、連続体ロボット１が目標経路Ｔから逸脱しない条件を充足する場合には、ブレーキ制御部３４はブレーキ部３４による駆動ユニット２の制動を行わない。この場合には、使用者が所望の移動速度（すなわち、要求移動速度）で連続体ロボット１を前後進動作させることができる。そして、この場合には、使用者が所望の速度で連続体ロボット１を前後進動作させても、連続体ロボット１は目標経路Ｔから逸脱しない。一方、使用者による連続体ロボット１（駆動ユニット２）の前後進動作の移動速度が、連続体ロボット１が目標経路Ｔから逸脱しない条件を充足しない場合には、ブレーキ制御部３４はブレーキ部３４を制御して駆動ユニット２を制動させる。これにより、連続体ロボット１を減速させる。このためこの場合には、連続体ロボット１の前後進動作の移動速度を、連続体ロボット１が目標経路Ｔから逸脱しない速度に留めることができる。そして、制御装置４は、このような処理を所定の制御周期で繰り返し実行する。このような構成によれば、連続体ロボット１を前後進動作させる際に、連続体ロボット１が目標経路Ｔから逸脱しないようにできる。さらに、前後進動作の移動速度の低下を最小限にとどめることができる。このように、第２の実施形態によれば、第１の実施形態と同様の効果を奏することができる。
【０１３２】
＜他の実施形態＞
前記各実施形態では、連続体ロボット１が第１湾曲部１１１と第２湾曲部１１２の２つの湾曲部を有する構成を例に示した。しかしながら、本発明は、３つ以上の湾曲部を有する連続体ロボット１の制御にも適用できる。湾曲部の数をＡとすると、遠位端湾曲部の位置ｓAが、ｎＬc≦ｓA≦（ｎ＋１）Ｌcの範囲にあるとき、近位端側からｉ （１≦ｉ≦Ａ）番目の湾曲部の位置は、（ｎ−Ａ＋ｉ）Ｌc≦ｓA≦（ｎ−Ａ＋ｉ＋１）Ｌcの範囲にある。したがって、次の数式（３０）と数式（３１）を用いて、ｉ番目の湾曲部の角度目標値の計算と追従可能速度ｖuiの計算を行えばよい。なお、数式（３０）と数式（３１）は、それぞれ数式（１８）と数式（２８）のｎをｎ−Ａ＋ｉで置き換えた式である。
【０１３３】
【数２５】
2018175602.tif 000027
【０１３４】
また、本発明は各湾曲部の長さがそれぞれ異なる連続体ロボット１に対しても適用可能である。さらに、前記各実施形態では、連続体ロボット１が距離Ｌc進むごとに遠位端湾曲部の目標角度ψの入力を要求したが、目標角度の入力を行うまでに連続体ロボット１が進む距離は湾曲部の長さと異なっていてもよい。遠位端湾曲部からｉ番目の湾曲部までの長さをＬiとすると、遠位端湾曲部の位置がｓAのとき、ｉ番目の湾曲部の位置はｓA−Ｌiとなる。また、遠位端湾曲部が距離Ｌd進むごとに目標角度ψの入力作業を行うとすると、角度記憶部４２はＬdの整数倍ごとの目標角度ψを記憶していく。このとき、角度制御部４１はｎiＬd≦ｓA−Ｌi≦（ｎi＋１）Ｌdを満たす整数ｎiを計算し、角度記憶部４２から目標角度ψni+1とψniを読み込む。そして、これらの目標角度を数式（２０）と数式（２１）に代入して遠位端角度θ1，θ2を計算し、移動駆動部３２の速度制御やブレーキ部３４の制動の判断を行う。
【０１３５】
また、前記各実施形態では、角度入力部５は、第２湾曲部１１２が現在位置より所定の距離Ｌc前進したときの目標角度ψの入力に用いられる構成を示したが、このような構成に限定されない。例えば、操作性を向上させるために一般的な内視鏡と同様に、現在位置における目標角度を入力するために用いられる構成であってもよい。この場合、連続体ロボットシステムにおいて駆動ユニット２を前進させるとき、角度制御部４１は、数式（１８）の角度ψn+1にψnを代入した次の数式（３２）を用いて、第２湾曲部１１２の遠位端角度θ2を制御する。
【０１３６】
【数２６】
2018175602.tif 000028
【０１３７】
この数式（３２）より、現在位置からの移動距離ｌ（０≦ｌ≦Ｌc）において第２湾曲部１１２の目標角度は一定となるため、第２角度制御モータ２１２を駆動しなくてもよい。このため、駆動ユニット速度計算・制御部４４は、第１角度制御モータ２１１に関する数式（２７）のみを用いて移動駆動部３２の制御を行う。また、ブレーキ制御部３４は、第１角度制御モータ２１１に関する数式（２７）のみを用いてブレーキ部３４の制御を行う。なお、現在位置での目標角度ψを入力する場合においても、前進時に入力した目標角度が角度記憶部４２に記憶されるため、後進時には前記各実施形態と同様に、数式（２７）と数式（２８）の両方を用いて、移動駆動部３２やブレーキ部３４を制御する。
【０１３８】
また、前記各実施形態では、連続体ロボット１が距離Ｌc進むごとに、使用者が目標角度ψを入力（設定）し、制御装置４がこの目標角度ψを取得したが、このような構成に限定されない。例えば、前進動作を行う前に連続体ロボット１が通過すべき目標経路Ｔが既知である場合には、距離Ｌc進むごとの入力（設定）や取得を省略できる。例えば、目標経路Ｔから目標角度ψを計算する方法としては、まず目標経路Ｔをそれぞれの長さがＬcである複数の区間に分割し、各区間の端点おける経路の接線を計算し、各端点における接線とＸ1軸のなす角度を目標角度ψとする方法が適用できる。この場合、角度記憶部４２に目標経路Ｔをあらかじめ記憶させておき、駆動ユニット速度計算・制御部４４やブレーキ制御部３４が前記計算によって目標角度ψを計算するという構成が適用できる。また、使用者が目標経路Ｔの各区間の端点の目標角度を角度記憶手段に記憶させる構成であってもよい。このような構成によれば、使用者は前後進動作中に目標角度ψの入力を行わなくてもよくなる。
【０１３９】
さらに、本発明は、複数の姿勢変更部（角度制御モータ）を用いて１つの湾曲部を立体的に駆動する連続体ロボットに対しても適用可能である。例えば、連続体ロボットシステムが軟性内視鏡に適用される場合、挿抜経路である食道や気管支や肺や大腸など人体の器官は立体的に湾曲している部分を有している。このため、これらの器官に沿って連続体ロボット１を挿入・抜去するためには、複数の姿勢変更部（角度制御モータ）を用いて各湾曲部を立体的に駆動しながら先頭追従動作を行うことが望ましい。この場合、駆動ユニット速度計算・制御部４４は、全ての姿勢変更部について、数式（２９）を充足するか否かを判断し、判断結果に応じて目標移動速度を設定する。また、ブレーキ制御部３４は、全ての姿勢変更部について目標角速度が最大値以下となるようにブレーキ部３４を制御する。例えば、前記第１の実施形態と同様に、駆動ユニット２の速度から湾曲部が目標経路Ｔから逸脱しないという条件を充足するためのモータの角速度を計算する。そして、少なくとも一つのモータについて、要求される角速度が最大角速度を超過するときには、ブレーキ部３４を制御して駆動ユニット２を制動する。
【０１４０】
また、本発明は、モータの最大回転速度に加えて、モータの加速度の上限によって生じる湾曲部の応答の遅れを考慮して連続体ロボット１の移動速度を制御することも可能である。連続体ロボット１の最大速度に関する条件を表す数式（３１）を時間で微分すると、次の数式（３３）が得られる。数式（３３）の左辺は連続体ロボット１（駆動ユニット２）の加速度であり、左辺のａmaxはモータの最大角加速度である。
【０１４１】
【数２７】
2018175602.tif 000029
【０１４２】
駆動ユニット速度計算・制御部４４は、センサ２４から取得した連続体ロボット１の移動速度ｖから連続体ロボット１の加速度を計算する。そして、駆動ユニット速度計算・制御部４４は、計算した加速度が数式（３３）を充足しない場合には、連続体ロボット１の目標移動速度を最大速度ｖmaxより低い値に設定する。これにより、モータを最大角加速度以下で駆動しながら先頭追従制御を行うことが可能となる。また、計算した加速度が数式（３３）を充足しない場合には、ブレーキ制御部３４は、ブレーキ部３４を作動させることで、モータを最大角加速度以下で駆動しながら先頭追従制御を行うことが可能となる。
【０１４３】
（その他の適用例）
前記各実施形態では、連続体ロボットシステムが軟性内視鏡に適用される例を示したが、本発明に係る例御システムの適用対象は軟性内視鏡に限定されない。例えば、配管内部や狭隘な空間に挿入して作業を行う産業用の連続体ロボットの制御にも適用できる。この場合、連続体ロボットの遠位端に設けられているカメラを、適用対象に応じて適宜変更すればよい。例えば、カメラのほかに、各種工具や機器が設けられる構成が適用できる。
【０１４４】
（その他の実施例）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１４５】
さらに、前記実施形態では、ＣＰＵとＲＯＭなどの記憶媒体とＲＡＭとを有するコンピュータがプログラムを実行することによりコンピュータが制御装置の各部として機能する構成を示したが、このような構成に限定されない。例えば、前記制御装置の各部が、それぞれ個別のハードウェアによって構築される構成であってもよい。
【産業上の利用可能性】
【０１４６】
本発明は、連続体ロボットシステム、連続体ロボットシステムの制御方法に好適な技術である。そして、本発明によれば、連続体ロボットを前進や後進させる際に、連続体ロボットが目標経路から逸脱しないようにできる。
【図１】
2018175602.tif 000030
【図２】
2018175602.tif 000031
【図３】
2018175602.tif 000032
【図４】
2018175602.tif 000033
【図５】
2018175602.tif 000034
【図６】
2018175602.tif 000035
【図７】
2018175602.tif 000036
【図８】
2018175602.tif 000037
【図９】
2018175602.tif 000038
【図１０】
2018175602.tif 000039
【図１１】
2018175602.tif 000040