JP6904767B2 - 連続体ロボットシステムの制御装置、連続体ロボットシステムの作動方法、プログラム - Google Patents

Description

本発明は、連続体ロボットシステムの制御装置、連続体ロボットシステムの作動方法、プログラムに関する。

特許文献１には、連続体ロボットの湾曲部の姿勢を制御する制御器を備え、制御器が連続体ロボットの進行方向の先頭に位置する湾曲部が通過した経路に沿うように後続の湾曲部を制御する内視鏡が開示されている。以下、このような姿勢制御を「先頭追従制御」と称する。先頭追従制御を行う制御器を有する内視鏡によれば、内視鏡の挿入対象である被検者の体腔への挿入時には、使用者は挿入方向の先頭に位置する湾曲部の姿勢を操作するだけで、後続する湾曲部は、被検者の体組織と接触しないように自動的に制御される。また、抜去時においては、各湾曲部の姿勢は挿入時に通過した経路に沿うように自動的に制御されるため、使用者は湾曲部の姿勢を操作しなくてもよい。このように、先頭追従制御を用いることによって、連続体ロボットの挿入・抜去時における使用者の負担を軽減することができる。

米国特許出願公開第２００５／０２０９５０９号明細書

先頭追従制御では、目標経路に沿うように連続体ロボットの湾曲部の姿勢を変化させることから、高速に挿入・抜去動作を行うためには、湾曲部の姿勢を高速に変化させる必要がある。しかしながら、一般に連続体ロボットの姿勢を変化させるアクチュエータや駆動伝達機構の応答速度や駆動速度には上限が存在する。このため、アクチュエータに駆動指令を与えてから（アクチュエータが動作開始してから）湾曲部の姿勢が目標姿勢と一致するまでには、ある程度の時間を要する。そのため、連続体ロボットを高速で挿入・抜去させると、湾曲部の姿勢の変化が目標経路の形状に追従できず、アクチュエータが目標経路から逸脱するおそれがある。

本発明が解決しようとする課題は、連続体ロボットを前進や後進させる際に、連続体ロボットが目標経路から逸脱しないようにすることである。

本発明は、長尺方向に直列に設けられた、それぞれが湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、前記複数の湾曲部のそれぞれを湾曲させることによりそれぞれの姿勢を変更する姿勢変更部と、を有する連続体ロボットシステムの制御装置であって、前記制御装置は、前記移動駆動部を制御する移動駆動制御手段と、前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、を有し、前記移動駆動制御手段は、前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下にすることを特徴とする。

本発明によれば、連続体ロボットを前進動作や後進動作させる際に、連続体ロボットが目標経路から逸脱しないようにできる。

第１の実施形態に係る連続体ロボットシステムの構成例を示す模式図である。 連続体ロボットの構成例を示す模式図である。 連続体ロボットの第１湾曲部のモデルを示す模式図である。 連続体ロボットの第２湾曲部のモデルを説明する模式図である。 制御装置の構成例を示すブロック図である。 連続体ロボットの先頭追従制御の例を示す図である。 制御装置が実行する処理の例を示すフローチャートである。 第２の実施形態に係る連続体ロボットシステムの構成例を模式的に示す図である。 本実施形態に係る制御装置の構成例を示すブロック図である。 制御装置が実行する処理の例を示すフローチャートである。 制御装置が実行する処理の例を示すフローチャートである。

以下に、本発明の各実施形態について、図面を参照して詳細に説明する。本発明の実施形態では、連続体ロボットシステムが軟性内視鏡に適用される例を示す。近年、患者の負担を低減し治療・検査後のＱＯＬを向上させるための低侵襲医療が注目を集めており、低侵襲医療の例として、内視鏡を用いた手術・検査が挙げられる。例えば、腹腔鏡手術は従来の開腹手術と比べて手術創を小さくすることが可能となるため、術後に必要な入院期間を短縮できるだけでなく、美容上も優れているというメリットがある。低侵襲医療に用いられる内視鏡は硬性内視鏡と軟性内視鏡に大別される。このうち、軟性内視鏡は、挿入部が操作者の操作により湾曲可能な部材で構成されているため、内視鏡先端の湾曲角度を操作することで広い範囲をくまなく観察できる。

＜第１の実施形態＞
（連続体ロボットシステムの構成の例）
まず、第１の実施形態に係る連続体ロボットシステムの構成例について説明する。図１は、第１の実施形態に係る連続体ロボットシステムの構成例を示す模式図である。なお、図１（ａ）は、連続体ロボットシステムの全体構成を示し、図１（ｂ）は連続体ロボット１と駆動ユニット２とリニアガイド３と移動駆動部３２とを示す。図１に示すように、第１の実施形態に係る連続体ロボットシステムは、連続体ロボット１と、駆動ユニット２と、リニアガイド３と、移動駆動部３２と、制御装置４と、角度入力部５と、ガイド操作部７と、カメラ６とを含んでいる。ただし、連続体ロボットシステムに含まれる機器の種類や構成は、図１に示す例に限定されるものではない。

連続体ロボット１は、長尺方向に直列に連続するように設けられている複数の湾曲部を有している。複数の湾曲部のそれぞれは、円弧状に湾曲可能である。そして、それぞれの湾曲部は、駆動ユニット２に設けられている複数の姿勢変更部（アクチュエータ）のそれぞれによって変形駆動されて姿勢が変化する。なお、連続体ロボット１の詳細な構成例については後述する。本実施形態では、直列に連続するように設けられている複数の湾曲部のうち、駆動ユニット２から最も遠い湾曲部を「遠位端湾曲部」と称し、遠位端湾曲部の反対側であって駆動ユニット２に最も近い湾曲部を「近位端湾曲部」と称する。連続体ロボット１の全体およびそれぞれの湾曲部についても、長尺方向について駆動ユニットから遠い側の端部を「遠位端」と称し、遠位端の反対側の端部であって駆動ユニット２に近い側の端部を「近位端」と称する。連続体ロボット１は、長尺方向に移動可能であり、移動駆動部３２の駆動力によって、駆動ユニット２とともに前後進動作する。本実施形態では、遠位端湾曲部を先頭とする長尺方向の移動を前進動作とし、近位端湾曲部を先頭とする長尺方向の移動を後進動作とする。

遠位端湾曲部の遠位端には、カメラ６が設けられている。なお、カメラ６の構成は特に限定されるものではなく、軟性内視鏡に用いられている公知の各種カメラが適用できる。

駆動ユニット２は、リニアガイド３の長尺方向に往復移動可能である。移動駆動部３２は、制御装置４の制御にしたがって駆動ユニット２をリニアガイド３に沿って前後進動作させる。そして、駆動ユニット２が移動駆動部３２の駆動力によって前後進動作（本例では、直線移動）することにより、連続体ロボット１がその長尺方向に前後進動作する。なお、リニアガイド３には長尺方向の位置を示すスケール３１が設けられており、駆動ユニット２にはスケール３１を検出するセンサ２４と図略の速度・位置計算部が設けられている。駆動ユニット２の速度・位置計算部は、センサ２４によるスケール３１の検出結果から駆動ユニット２の前後進動作の移動速度と現在位置を計算し、制御装置４に出力（送信）する。なお、移動駆動部３２の構成は特に限定されるものではなく、例えば公知の各種リニアアクチュエータなどが適用できる。

ガイド操作部７は、連続体ロボットシステムの使用者（操作者）が操作する機器である。ガイド操作部７には、制御装置４に前後進動作の指令を送信するための操作部材である前後進ボタン７１と、駆動ユニット２の前後進動作の要求移動速度ｖ_refを入力（設定）するためのボリューム７２とが設けられている。「要求移動速度」とは、使用者が要求する連続体ロボット１（駆動ユニット２）の前後進動作の移動速度をいう。ガイド操作部７は、前後進ボタン７１の操作に応じて駆動ユニット２の前後進動作の指令を移動駆動部３２に出力し、ボリューム７２の操作に応じて要求移動速度ｖ_refを移動駆動部３２に出力する。なお、ガイド操作部７の具体的な構成は特に限定されるものではない。ガイド操作部７は、使用者が移動駆動部３２に駆動ユニット２の前後進動作を指令するための操作と、駆動ユニット２の要求移動速度ｖ_refの入力（設定）操作とができる構成であればよい。

角度入力部５は、使用者が連続体ロボット１の遠位端湾曲部の遠位端角度を入力（設定）するための機器である。「遠位端湾曲部の遠位端角度」とは、連続体ロボット１が姿勢変化（角度変化）する平面上において、「遠位端湾曲部の遠位端を通過しその中心線に直角な直線」と「近位端湾曲部の近位端を通過しその前後進動作の移動方向に直角な直線」とがなす角度をいうものとする。角度入力部５には、例えば、遠位端湾曲部の遠位端角度を入力（設定）するための操作部材であるレバー５１が設けられている。使用者はレバー５１の操作により遠位端湾曲部の遠位端角度を入力（設定）でき、入力された遠位端湾曲部の遠位端角度は制御装置４に出力（送信）される。なお、角度入力部５の具体的な構成は特に限定されるものではない。角度入力部５は、遠位端湾曲部の目標姿勢である遠位端角度を入力でき、入力された遠位端角度を制御装置４に送信できる構成であればよい。

制御装置４は、ガイド操作部７から取得した前後進動作の指令および要求移動速度ｖ_refと、角度入力部５から取得した遠位端湾曲部の遠位端角度と、駆動ユニット２の速度・位置計算部による位置および速度の計算結果とに基づいて、駆動ユニット２を駆動する。なお、制御装置４の構成例および制御の例については後述する。

（連続体ロボットの構成例）
次に、連続体ロボット１の構成例について説明する。図２は、連続体ロボット１の構成例を示す模式図である。本実施形態に係る連続体ロボット１は、複数の湾曲部の例として、第１湾曲部１１１と第２湾曲部１１２との２つの湾曲部を有している。本実施形態では、第１湾曲部１１１が近位端湾曲部であるものとし、第２湾曲部１１２が遠位端湾曲部であるものとする。第１湾曲部１１１と第２湾曲部１１２は、円弧状に湾曲可能であり、互いに独立して姿勢の変更が可能である。

第１湾曲部１１１の遠位端には第１ワイヤ固定部２６１が設けられており、この第１ワイヤ固定部２６１には、駆動伝達機構である第１ワイヤ２３１の一方の端部が固定されている。第１ワイヤ２３１は、第１湾曲部１１１に沿って設けられている。そして、第１ワイヤ２３１の他方の端部は、駆動ユニット２の内部に引き込まれており、駆動ユニット２の内部に設けられている第１プーリ２２１に連結されている。第１プーリ２２１は、駆動ユニット２の内部に設けられている姿勢変更部の例である第１角度制御モータ２１１の駆動力によって回転する。本実施形態では、第１プーリ２２１と第１角度制御モータ２１１の出力軸２１３とに第１ベルト２１４が巻き掛けられており、第１角度制御モータ２１１の出力軸２１３の回転がこの第１ベルト２１４を介して第１プーリ２２１に伝達されるように構成されている。

同様に、第２湾曲部１１２の遠位端には第２ワイヤ固定部２６２が設けられており、第２ワイヤ固定部２６２には、駆動力伝達機構である第２ワイヤ２３２の一方の端部が固定されている。第２ワイヤ２３２は、第１湾曲部１１１と第２湾曲部１１２に沿って設けられており、長尺方向の中間部において第１ワイヤ固定部２６１に設けられている孔を通過している。そして、第２ワイヤ２３２の他方の端部は、駆動ユニット２の内部に引き込まれており、駆動ユニット２の内部に設けられている第２プーリ２２２に連結されている。第２プーリ２２２は、駆動ユニット２の内部に設けられている姿勢変更部の例である第２角度制御モータ２１２の回転駆動力によって回転する。本実施形態では、第２プーリ２２２と第２角度制御モータ２１２の出力軸２１５とに第２ベルト２１６が巻き掛けられており、第２角度制御モータ２１２の出力軸２１５の回転がこの第２ベルト２１６を介して第２プーリ２２２に伝達されるように構成されている。

このような構成であると、姿勢変更部の例である第１角度制御モータ２１１の出力軸２１３の回転角度に応じて、第１湾曲部１１１の姿勢が変化する。同様に、第２角度制御モータ２１２の出力軸２１５の回転角度に応じて第２湾曲部１１２の姿勢が変化する。このように、第１湾曲部１１１と第２湾曲部１１２とはいずれも湾曲可能であり、それぞれ姿勢変更部の例である第１角度制御モータ２１１と第２角度制御モータ２１２の動作によって、互いに独立して円弧状に湾曲させることができる。なお、第１角度制御モータ２１１の回転動力を第１プーリ２２１に伝達するための駆動力伝達機構と、第２角度制御モータ２１２の回転動力を第２プーリ２２２に伝達するための駆動力伝達機構は、前記構成に限定されるものではない。要は、第１プーリ２２１の回転角度が第１角度制御モータ２１１の出力軸２１３の回転角度から一義的に定まる構成であり、第２プーリ２２２の回転角度が第２角度制御モータ２１２の出力軸２１５の回転角度から一義的に定まる構成であればよい。

なお、本実施形態では、連続体ロボット１が第１ワイヤ２３１と第２ワイヤ２３２を有する構成を例に示すが、これは説明の便宜上簡略化したものであり、これら以外のワイヤを有していてもよい。例えば、第２ワイヤ２３２のほかに、第１湾曲部１１１と第２湾曲部１１２に沿って設けられ、第１ワイヤ固定部２６１と第２ワイヤ固定部２６２の両方に固定されている固定ワイヤを有していてもよい。このような構成であれば、それぞれの湾曲部を３次元的に湾曲させることができる。また、図２に示す構成では、第１ワイヤ固定部２６１が第２ワイヤ固定部２６２よりも大径となっているが、この構成に限られない。たとえば、第１ワイヤ固定部２６１が第２ワイヤ固定部２６２とを同径として、第２ワイヤ固定部２６２に固定された第２ワイヤ２３２が、第１ワイヤ固定部に設けられた空孔を挿通するようにしてもよい。さらに、第１ワイヤ２３１は、それぞれ独立に駆動される複数のワイヤとして構成してもよい。第２ワイヤ２３２も同様である。

（連続体ロボットのモデル）
ここで、連続体ロボット１の先頭追従制御と駆動ユニット２の前後進動作の制御に用いるためのモデルについて説明する。本実施形態では、第１湾曲部１１１の姿勢の例である遠位端角度θ₁と第１角度制御モータ２１１の出力軸２１３の回転角度の関係と、第２湾曲部１１２の姿勢の例である遠位端角度θ₂と第２角度制御モータ２１２の出力軸２１５の回転角度との関係とを、連続体ロボット１のモデルを用いて規定する。そして、制御装置４は、モデルを用いて規定されたこれらの関係を用いて、連続体ロボット１の先頭追従制御と、駆動ユニット２の前後進動作の速度制御を行う。

モデル化に当たり、本実施形態で制御対象となる連続体ロボット１は、以下の（１）〜（４）の４つ条件を充足するものとする。
（１） 第１湾曲部１１１の曲率半径はその長尺方向の全長にわたって一定であり、第２湾曲部１１２の曲率半径もその長尺方向の全長にわたって一定である。
（２） 第１湾曲部１１１の中立面Ｎ₁は第１湾曲部１１１の中心に位置しており、第２湾曲部１１２の中立面Ｎ₂は第２湾曲部１１２の中心に位置している。
（３） 第１ワイヤ２３１と第２ワイヤ２３２は伸縮しない（伸縮を考慮しない）。
（４） 第１湾曲部１１１と第２湾曲部１１２の長尺寸法は等しい。

まず、第１湾曲部１１１のモデルについて説明する。図３は、第１湾曲部１１１のモデルを示す模式図である。図３に示すように、第１湾曲部１１１の近位端を原点Ｏ₁とし、駆動ユニット２の前進動作の移動方向をＺ₁軸とし、第１湾曲部１１１が姿勢変化（角度変化）する平面上においてＺ₁軸と直角な軸をＸ₁軸とする。第１湾曲部１１１の遠位端角度θ₁は、第１湾曲部１１１の遠位端を通過し中心線（中立面Ｎ₁）に直角な直線とＸ₁軸とのなす角度である。φ₁は、第１角度制御モータ２１１の出力軸２１３の回転角度を表す。ｒ₁は、第１湾曲部１１１の中立面Ｎ₁と第１ワイヤ２３１との距離を表す。前記条件（１）により、第１湾曲部１１１の中立面Ｎ₁は円弧であり、円弧の中心角は第１湾曲部１１１の遠位端角度θ₁と等しい。また、条件（２）より、中立面Ｎ₁の長さをＬ_c、円弧の曲率中心と第１湾曲部１１１の中立面Ｎ₁との距離をＲ₁とすると、第１湾曲部１１１の遠位端角度θ₁は、次の数式（１）で表される。

この場合、第１ワイヤ２３１のうち、第１湾曲部１１１の中立面Ｎ₁の外側に位置する部分（説明の便宜上、外側部分２３１１と称する）の長さＬ_1oは、前記条件（３）より、次の数式（２）で表される。

第１湾曲部１１１の遠位端角度θ₁が０のとき、第１湾曲部１１１の中立面Ｎ₁の長さＬ_cと第１ワイヤ２３１の長さＬ_1oは一致する。このため、第１湾曲部１１１の遠位端角度を０からθ₁まで変化させるために必要な第１ワイヤ２３１の外側部分２３１１の駆動量ΔＬ_1oは、次の数式（３）で表される。

同様に、第１ワイヤ２３１のうちの第１湾曲部１１１の中立面Ｎ₁の内側（曲率中心に近い側）に位置する部分（説明の便宜上、内側部分２３１２と称する）の長さＬ_1iは、次の数式（４）で表される。

したがって、第１湾曲部１１１の遠位端角度を０からθ₁まで変化させるために必要な第１ワイヤ２３１の内側部分２３１２の駆動量ΔＬ_1iは、次の数式（５）で表される。

数式（３）と数式（５）より、第１ワイヤ２３１の外側部分２３１１と内側部分２３１２の駆動量は、大きさが等しく方向が反対である。第１角度制御モータ２１１の出力軸２１３の回転角度を０からφ₁に変化させた場合、第１ワイヤ２３１の外側部分２３１１の駆動量ΔＬ_1oと内側部分２３１２の駆動量ΔＬ_1iは、次の数式（６）で表される。なお、ｒ_oは第１角度制御モータ２１１の出力軸２１３の半径である。

これらより、第１湾曲部１１１の遠位端角度θ₁と第１角度制御モータ２１１の出力軸２１３の回転角度φ₁の関係は、次の数式（７）で表される。

次に、第２湾曲部１１２の遠位端角度θ₂と第２角度制御モータ２１２の出力軸２１５の回転角度φ₂との関係について説明する。図４は、連続体ロボット１の第２湾曲部１１２のモデルを説明する模式図である。なお、図４では、第１角度制御モータ２１１と第１プーリ２２１を省略している。第２湾曲部１１２の遠位端角度θ₂は、第２湾曲部１１２の遠位端を通過し中心線（中立面Ｎ₂）に直角な直線とＸ₁軸とのなす角度である。図４に示すように、第２湾曲部１１２の近位端を原点Ｏ₂とし、第２湾曲部１１２の長尺方向をＺ₂軸とし、第２湾曲部１１２が駆動する平面上においてＺ₂軸に直角な軸をＸ₂軸とする。なお、本実施形態では、第１湾曲部１１１と第２湾曲部１１２が同じ平面上で姿勢変化するため、Ｘ₁−Ｚ₁平面とＸ₂−Ｚ₂平面は一致している。

前記条件（２）と（３）によれば、第２湾曲部１１２の曲率半径Ｒ₂を変更しても、第１湾曲部１１１の中立面Ｎ₁および第１ワイヤ２３１の長さは変化しない。このため、第２湾曲部１１２の曲率半径Ｒ₂を変更しても、第１湾曲部１１１について前記数式（７）が成立する。また、前記条件（１）より、第１湾曲部１１１と同様に、第２湾曲部１１２の中立面Ｎ₂も円弧となる。前記条件（４）より、第２湾曲部１１２の長さは中立面Ｎ₂の長さＬ_cと等しくなる。このため、Ｘ₁軸に対する第２湾曲部１１２の遠位端角度をθ₂とし、中立面Ｎ₂と円弧の中心との距離をＲ₂とすると、次の数式（８）が成立する。なお、数式（８）の左辺は、第２湾曲部１１２の円弧の中心角である。

第２ワイヤ２３２のうち、その中立面Ｎ₂から曲率の半径方向外側に位置する部分（説明の便宜上、外側部分と称する）のうち、第２湾曲部１１２に沿う部分２３２１の長さＬ_22oは、次の数式（９）で表される。ｒ₂は、第２湾曲部１１２の中立面Ｎ₂と第２ワイヤ２３２との距離である。

第２ワイヤ２３２のうち、第１湾曲部１１１の中立面Ｎ１の曲率の半径方向外側に位置する部分（説明の便宜上、外側部分と称する）のうち、第１湾曲部１１１に沿う部分２３２３の長さＬ_21oは、数式（２）に示す第１湾曲部１１１と同様に、次の数式（１０）で表される。

このため、第２ワイヤ２３２の外側部分２３２１，２３２３の和Ｌ_2oは、数式（９）と数式（１０）とから、次の数式（１１）で表される。

したがって、第２湾曲部１１２の遠位端角度がθ₂のときの第２ワイヤ２３２の外側部分の駆動量ΔＬ_2oは、次の数式（１２）で表される。

同様に、第２ワイヤ２３２のうちの中立面Ｎ₂より曲率の半径方向内側に位置する部分（以下、内側部分２３２２，２３２４と称する）の長さＬ_22iとＬ_21iは、それぞれ、次の数式（１３）と数式（１４）で表される。そして、第２ワイヤ２３２のうちの内側部分２３２２，２３２４の駆動量ΔＬ_2iは、数式（１５）で表される。

したがって、第２角度制御モータ２１２の出力軸２１５の回転角度を０からφ₂に変化させた場合、出力軸２１５の回転角度φ₂と第２湾曲部１１２の遠位端角度θ₂との関係は、次の数式（１６）で表される。なお、ｒ_oは、第２角度制御モータ２１２の出力軸２１５の半径である。

以上のとおり、連続体ロボット１のモデルから、第１角度制御モータ２１１の出力軸２１３の回転角度φ₁と第１湾曲部１１１の遠位端角度θ₁との関係が規定される。同様に、第２角度制御モータ２１２の出力軸２１５の回転角度φ₂と第２湾曲部１１２の遠位端角度θ₂との関係が規定される。そして、制御装置４は、これらの関係を用いて、連続体ロボット１の先頭追従制御と、駆動ユニット２の前後進動作の制御を行う。

なお、上記説明では、長尺方向に直列に連結するように設けられている２つの湾曲部を有する連続体ロボット１を示したが、湾曲部の数は限定されない。長尺方向に直列に連結するように設けられている３つ以上の湾曲部を有する連続体ロボットについても同様に、角度制御モータの出力軸の回転角度と遠位端湾曲部の遠位端角度との関係を規定できる。すなわち、前記条件（１）〜（４）によれば、第ｉ湾曲部の遠位端角度θ_iは、第ｉ湾曲部を駆動する第ｉ角度制御モータの出力軸の回転角度φ_iにのみ依存する。そして、第ｉ角度制御モータの出力軸の回転角度φ_iと第ｉ湾曲部の遠位端角度θ_iとの関係は、第ｉ湾曲部の中立面Ｎ_iと第ｉ湾曲部を駆動するワイヤとの距離ｒ_iを用いると、次の数式（１７）で表される。なお、ｉは正の整数であり、近位端湾曲部をｉ＝１とする。

なお、本実施形態では、第１角度制御モータ２１１から第１湾曲部１１１の間と、第２角度制御モータ２１２から第２湾曲部１１２の間に減速機構が設けられない構成を例に示した。すなわち、第１角度制御モータ２１１の出力軸２１３の外周面の円周方向の移動量（回転角度×半径）と第１ワイヤ２３１の駆動量とが同じである。また、第２角度制御モータ２１２の出力軸２１５の外周面の円周方向の移動量（回転角度×半径）と第２ワイヤ２３２の駆動量とが同じである。ただし、このようなモデルに限定されない。例えば、第１角度制御モータ２１１から第１湾曲部１１１の間と、第２角度制御モータ２１２から第２湾曲部１１２の間に減速機構や倍力機構が設けられる構成であってもよい。この場合、数式（７）と数式（１６）と数式（１７）の右辺のそれぞれに、減速機構の減速比や倍力機構の倍力比に応じた係数を乗じればよい。

（制御装置）
次に、制御装置４について説明する。図５は、制御装置４の構成例を示すブロック図である。図５に示すように、制御装置４は、角度制御部４１と、角度記憶部４２と、移動駆動制御手段の例である駆動ユニット速度計算・制御部４４とを有する。角度記憶部４２は、駆動ユニット２から取得した駆動ユニット２の位置ｚと、角度入力部５から取得した第２湾曲部１１２の目標姿勢の例である遠位端の目標角度ψを記憶する。角度制御部４１は、第ｉ湾曲部（ｉ＝１，２）の位置がｓ_iとなるときの目標角度ψ_i（目標姿勢）を角度記憶部４２から読み出し、連続体ロボット１が先頭追従制御を行うように第１湾曲部１１１と第２湾曲部１１２のそれぞれの遠位端角度θ₁，θ₂を制御する。駆動ユニット速度計算・制御部４４は、駆動ユニット２の前後進動作の目標移動速度を計算する。そして、駆動ユニット速度計算・制御部４４は、駆動ユニット２の前後進動作の移動速度が計算した目標移動速度となるように移動駆動部３２を制御する。

制御装置４は、ＣＰＵとＲＯＭなどの記憶媒体とＲＡＭとを有するコンピュータを有している。ＲＯＭなどの記憶媒体には、駆動ユニット２を制御するためのコンピュータプログラムがあらかじめ格納されている。制御装置４のコンピュータのＣＰＵは、ＲＯＭなどの記憶媒体に格納されているこのコンピュータプログラムを読み出し、ＲＡＭをワークエリアとして用いて実行する。これにより、コンピュータが制御装置４の前記各部として機能し、後述する連続体ロボット１の制御と駆動ユニット２の速度制御が実現する。

（先頭追従制御）
ここで、制御装置４による連続体ロボット１の先頭追従制御について説明する。図６は、湾曲した部分を有する経路に沿って連続体ロボット１を前進動作させる際の先頭追従制御する例を示す。連続体ロボット１を挿入・抜去させる経路（以下、挿抜経路と称する）は連続体ロボット１が進行するのに十分な幅を有しているものとし、図６において、太い実線がこの挿抜経路を規定する境界を示している。また、図６における破線は、連続体ロボット１の目標経路を示しており、制御装置４は連続体ロボット１の各湾曲部がこの目標経路Ｔに沿って挿入・抜去されるように（前後進動作するように）制御する。なお、目標経路Ｔは、挿抜経路の形状などに応じて使用者等が適宜決定するものである。例えば、軟性内視鏡であれば、使用者がカメラ６により撮影された画像に基づいて目標経路Ｔを設定したり、ＭＲＩ検査により挿抜経路を把握して目標経路Ｔを設定したりする。

図６に示すように、挿抜経路は曲がっている部分を有しており、このため目標経路Ｔも曲がっている部分を有している。制御装置４は、挿抜経路の曲がっている部分においても連続体ロボット１の第１湾曲部１１１と第２湾曲部１１２が目標経路Ｔから逸脱しないように、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂を制御する。換言すると、連続体ロボット１の複数の湾曲部が挿抜経路の外部に逸脱しないように、遠位端角度θ₁，θ₂を制御する。目標経路Ｔに沿って座標軸ｓをとると、連続体ロボット１の各湾曲部が目標経路Ｔ上にあるとき、駆動ユニット２のｚ方向の移動量と各湾曲部のｓ方向の移動量が一致する。そこで、本実施形態では、位置ｓを用いて各湾曲部の前後進方向の位置を表す。

図６（ａ）は、連続体ロボット１の初期状態の例を示す。図６（ａ）に示すように、初期状態における第２湾曲部１１２の遠位端角度θ₂をψ_nとし、位置ｓ₂をｎＬ_cとする。また、初期状態における第１湾曲部１１１の遠位端角度θ₁をψ_n-1とし、位置ｓ₁を（ｎ−１）Ｌ_cとする。なお、ｎは正の整数であり、Ｌ_cは第１湾曲部１１１および第２湾曲部１１２のそれぞれの長尺方向寸法（中立面Ｎ₁，Ｎ₂の長尺方向寸法）である。

連続体ロボット１を前進動作させる場合には、使用者は角度入力部５を用いて、第２湾曲部１１２が位置（ｎ＋１）Ｌ_cに到達するときの第２湾曲部１１２の目標姿勢の例である遠位端の目標角度をψ_n+1を入力する。角度記憶部４２は、各湾曲部の現在の位置（ｎ−１）Ｌ_c、ｎＬ_cと目標位置（ｎ＋１）Ｌ_c、遠位端の現在の角度ψ_n-1、ψ_nと遠位端の目標角度ψ_n+1をそれぞれ記憶する。角度制御部４１は、角度記憶部４２に記憶された角度ψ_n-1、ψ_n、ψ_n+1を読み込む。そして、角度制御部４１は、次に示す数式（１８）と数式（１９）を用いて、連続体ロボット１が初期位置から距離ｌ （０≦ｌ≦Ｌ_c） 移動したときの第１湾曲部１１１と第２湾曲部１１２のそれぞれの目標姿勢の例である遠位端角度θ₁，θ₂を計算する。

図６（ｂ）は、連続体ロボット１が図６（ａ）に示す初期状態から距離Ｌ_c（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）進んだ状態を示す。図６（ｂ）に示すように、移動駆動部３２によって駆動ユニット２が前進すると、それに伴って連続体ロボット１が距離Ｌ_c前進する。そしてこの場合、角度制御部４１は、数式（７）と数式（１６）を用いて、遠位端角度θ₁，θ₂から、第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の目標回転角度φ₁，φ₂を計算する。そして、角度制御部４１は、計算した目標回転角度φ₁，φ₂となるように、第１角度制御モータ２１１と第２角度制御モータ２１２を制御する。数式（１８）と数式（１９）より明らかなように、駆動ユニット２がｌ＝０からｌ＝Ｌ_cへ前進すると、第１湾曲部１１１の遠位端角度θ₁はψ_n-1からψ_nへ、第２湾曲部１１２の遠位端角度θ₂はψ_nからψ_n+1へ、それぞれ連続的に変化する。従って、図６（ｃ）に示すようｓ₁＝ｎＬ_cにおける第１湾曲部１１１の姿勢は、同じ位置における第２湾曲部１１２の姿勢と一致する。

連続体ロボット１が距離Ｌ_c（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）前進するごとに、角度制御部４１は同様の処理を実行する。すなわち、目標位置（ｎ＋２）Ｌ_cにおける第２湾曲部１１２の目標角度をψ_n+2を入力して連続体ロボット１を前進させると、角度制御部４１は、次の数式（２０）と数式（２１）を用いて遠位端角度θ₁，θ₂を計算する。そして、角度制御部４１は、これらと各湾曲部の遠位端角度が目標角度と一致するように第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度を制御する。なお、数式（２０）と数式（２１）は、それぞれ数式（１８）と数式（１９）中のｎをｎ＋１と置き換えたものである。

数式（１８）と数式（２１）が同じであることから明らかなように、第１湾曲部１１１は、第２湾曲部１１２がｓ₂＝ｎＬ_cからｓ₂＝（ｎ＋１）Ｌ_cへ前進するときに通過した経路を追従するように通過する。このように、先頭追従制御が実現する。

連続体ロボット１を後進動作させる際の制御は次のとおりである。角度制御部４１は前進動作時に通過した経路に沿うように、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂を制御する。説明の便宜上、後進動作を開始するときの第２湾曲部１１２の遠位端角度θ₂をψ_m+1とし、位置ｓ₂を（ｍ＋１）Ｌ_cとする。同様に、第１湾曲部１１１の遠位端角度θ₁をψ_mとし、位置ｓ₁をｍＬ_cとする。なお、ｍはｎ＜ｍを満たす正の整数を表す。

前述のように、角度記憶部４２は、前進動作時に入力された目標角度ψ_m-1、ψ_m、ψ_m+1を記憶している。そこで、角度制御部４１は、まず、角度記憶部４２から目標角度ψ_m-1、ψ_m、ψ_m+1を読み込む。そして、次に示す数式（２２）と数式（２３）を用いて遠位端角度θ₁，θ₂を計算し、これらと一致するように第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度を制御する。

そして、前進動作時と同様に、連続体ロボット１が距離Ｌ_c（第１湾曲部１１１および第２湾曲部１１２の長尺方向寸法と同じ距離）後進するごとに同様の制御を繰り返す。これにより、第１湾曲部１１１の通過した経路に沿って第２湾曲部１１２が後進動作を行う。このように、後進動作時においても、先頭追従制御が実行される。

（速度制御）
次いで、連続体ロボット１の前後進動作の速度制御について説明する。連続体ロボット１の挿入・抜去を短時間で行うには、駆動ユニット２をできるだけ高速に前後進させることが望ましい。しかしながら、第１角度制御モータ２１１と第２角度制御モータ２１２の回転速度には物理的な上限が存在する。このため、第１角度制御モータ２１１と第２角度制御モータ２１２が動作開始してから第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂が目標角度に一致するまでには、ある程度の時間を要する。したがって、連続体ロボット１を高速で前後進（すなわち、挿入・抜去）すると、第１湾曲部１１１や第２湾曲部１１２の遠位端角度θ₁，θ₂が目標角度に一致するよりも早く、連続体ロボット１が距離Ｌ_cの進行が完了することがある。そうすると、第１湾曲部１１１や第２湾曲部１１２が目標経路Ｔから逸脱して挿抜経路と外部との境界に接触するおそれがある。例えば、本実施形態に係る連続体ロボットシステムが軟性内視鏡に適用される場合には、連続体ロボット１が被検者の体組織の内面に接触するおそれがある。

そこで、制御装置４は、ボリューム７２により入力（設定）された要求移動速度ｖ_refで駆動ユニット２を前進動作または後進動作させた場合に、距離Ｌ_cの進行が完了する以前に第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂を目標角度に一致させることができるか否かを判断する。すなわち、要求移動速度ｖ_refで駆動ユニット２を前後進動作させた場合に、連続体ロボット１が目標経路Ｔから逸脱することなく距離Ｌ_c進行させることができるか否かを判断する。

本実施形態では、駆動ユニット速度計算・制御部４４は、連続体ロボット１の各湾曲部が目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値を計算する。本実施形態では、「湾曲部が目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値」を、「追従可能速度」と称する。そして、駆動ユニット速度計算・制御部４４は、設定された要求移動速度ｖ_refが計算した追従可能速度以下（目標経路Ｔから逸脱することなく前後進動作できる移動速度の最大値以下）であるか否かを判断する。設定された要求移動速度ｖ_refが計算した追従可能速度以下である場合には、連続体ロボット１を要求移動速度ｖ_refで前後進させても連続体ロボット１が目標経路Ｔから逸脱しないようにできることを意味している。そこでこの場合には、駆動ユニット速度計算・制御部４４は、連続体ロボット１を要求移動速度ｖ_refで前後進動作させても目標経路Ｔから逸脱することなく追従制御が可能であると判断する。そして、移動駆動部３２による駆動ユニット２（連続体ロボット１）の目標移動速度を要求移動速度ｖ_refに設定する。

一方、設定された要求移動速度ｖ_refが計算した追従可能速度を超える場合には、連続体ロボット１を要求移動速度ｖ_refで前後進させると、連続体ロボット１が目標経路Ｔから逸脱するおそれがあることを意味している。そこでこの場合には、駆動ユニット速度計算・制御部４４は、連続体ロボット１を要求移動速度ｖ_refで前後進動作させると連続体ロボット１が目標経路Ｔから逸脱するおそれがあると判断し、目標移動速度を計算した追従可能速度以下の移動速度に設定する。そして、駆動ユニット速度計算・制御部４４は、移動駆動部３２を制御し、このように設定した目標移動速度で駆動ユニット２を前後進させる。このような構成によれば、連続体ロボット１が目標経路Ｔから逸脱することを防止できる。

なお、連続体ロボット１の前後進の移動速度を低くし過ぎた場合であっても、連続体ロボット１が目標経路Ｔから逸脱するおそれがある。また、連続体ロボット１の前後進速度を低くすると、連続体ロボット１が目標経路Ｔから逸脱しないまでも、挿入・抜去に要する時間が長くなる。そこで、駆動ユニット速度計算・制御部４４は、目標移動速度の下限の閾値を設定し、設定した閾値以上の移動速度で駆動ユニット２が前後進するように移動駆動部３２を制御してもよい。この閾値は追従可能速度以下であればよく、適宜設定される。ただし、挿入・抜去時間の短縮の観点からは、この閾値はできるだけ高い値であることが好ましい。

具体的な制御の内容は次のとおりである。まず、連続体ロボット１が目標経路Ｔから逸脱することなく前後進動作できる移動速度、すなわち、連続体ロボット１が目標経路Ｔから逸脱することなく前後進動作できる移動速度の条件について説明する。数式（７）の両辺を時間で微分すると、次の数式（２４）が得られる。

また、式（１９）を時間で微分すると、駆動ユニット２（連続体ロボット１）の前進動作中はψ_nとψ_n-1が定数であることから、次の数式（２５）が得られる。

数式（２４）と数式（２５）とから、次の数式（２６）が得られる。

数式（２６）より、距離Ｌ_c移動する間に第１湾曲部１１１の遠位端角度θ₁を目標角度に一致させることができる移動速度ｖの最大値、すなわち、第１湾曲部１１１についての追従可能速度ｖ_u1は、次の数式（２７）で表される。なお、ω_maxは、第１角度制御モータ２１１の出力軸２１３の出力可能な最大角速度である。

連続体ロボット１の移動速度ｖが数式（２７）で計算される第１湾曲部１１１についての追従可能速度ｖ_u1以下である場合には、距離Ｌ_c移動する間に（移動が完了するまでに）、第１湾曲部１１１の遠位端角度θ₁を目標角度に一致させることができる。

同様に、数式（１６）と数式（１８）より、第２湾曲部１１２についての追従可能速度ｖ_u2は、次の数式（２８）で表される。連続体ロボット１の移動速度ｖが数式（２８）で計算される追従可能速度ｖ_u2以下である場合には、距離Ｌ_c移動する間に（移動が完了するまでに）、第２湾曲部１１２の遠位端角度θ₂を目標角度に一致させることができる。なお、ω_maxは、第２角度制御モータ２１２の出力軸２１５の出力可能な最大角速度である。

なお、数式（７）と数式（２３）、数式（１６）と数式（２２）を用いて同様の計算を行うと、連続体ロボット１（駆動ユニット２）が後進する際における第１湾曲部１１１と第２湾曲部１１２の追従可能速度ｖ_u1，ｖ_u2も、数式（２７）と数式（２８）で与えられることが明らかである。

したがって、連続体ロボット１を距離Ｌ_c移動させる間に第１湾曲部１１１と第２湾曲部１１２の両方が目標経路Ｔから逸脱しないようにするためには、連続体ロボット１の移動速度ｖが、第１湾曲部１１１と第２湾曲部１１２についての追従可能速度のうちの小さい方の値以下であればよい。ただし、数式（２７）と数式（２８）は、モデルを用いて導出した計算式であり、誤差が存在している場合がある。そして、連続体ロボット１のモデルに誤差が存在すると、連続体ロボット１が目標経路Ｔから逸脱するおそれがある。そこで、本実施形態では、モデルの誤差を考慮して、数式（２７）と数式（２８）で計算される追従可能速度ｖ_u1，ｖ_u2のうちの小さい方に係数αを乗じた値を計算する。この係数αの値は、０より大きく１以下である。なお、この係数αの具体的な値は、モデルに含まれる誤差の大きさに応じて適宜設定すればよい。例えば、係数αの値は、誤差が大きいほど小さく、誤差が小さいほど大きいことが好ましい。また、誤差が無視できる程度であれば、係数αを乗じなくてもよい（係数αの値を１にしてもよい）。

そして、駆動ユニット速度計算・制御部４４は、この値と連続体ロボット１（駆動ユニット２）の要求移動速度ｖ_refが数式（２９）に示す条件を充足するか否かを判断する。すなわち、要求移動速度ｖ_refが、それぞれの湾曲部についての追従可能速度のうちの最も小さい値に係数αを乗じた値以下（モデルの誤差を考慮した追従可能速度以下）であるか否かを判断する。

駆動ユニット速度計算・制御部４４は、数式（２９）が成立する場合には、連続体ロボット１の目標移動速度（すなわち、移動駆動部３２による駆動ユニット２の目標移動速度）を要求移動速度ｖ_refに設定する。一方、要求移動速度ｖ_refにつて数式（２９）が成立しない場合には、駆動ユニット２の目標移動速度を数式（２９）の右辺で計算される値に設定する。数式（２９）の右辺で計算される値は、それぞれの湾曲部についての追従可能速度のうちの最も小さい値に係数αを乗じた値（すなわち、モデルの誤差を考慮した追従可能速度）である。このように、駆動ユニット速度計算・制御部４４は、駆動ユニット２の目標移動速度を、追従可能速度以下に制限する（移動速度制限手段）。

駆動ユニット速度計算・制御部４４は、駆動ユニット２が距離Ｌ_c前後進動作するごとに、上記計算および制御を繰り返す。このような速度制御によれば、連続体ロボット１が目標経路Ｔから大きく逸脱しないようにできる。また、連続体ロボット１を、目標経路Ｔに追従可能な移動速度の最大速度で移動させることができる。このため、連続体ロボット１の挿抜経路への挿入・抜去に要する時間が長くなることを抑制できる。

（処理の例）
次に、制御装置４が実行する処理の例について説明する。図７は、制御装置４が実行する処理の例を示すフローチャートである。なお、この処理を実行するためのコンピュータプログラムは、あらかじめ制御装置４のコンピュータのＲＯＭなどの記憶媒体に格納されている。そして、コンピュータのＣＰＵはこのコンピュータプログラムをＲＯＭなどの記憶媒体から読み出し、ＲＡＭをワークエリアとして用いて実行する。これにより、コンピュータが制御装置４の前記各部として機能し、この処理が実現する。

ステップＳ１０１の「要求移動速度・目標角度入力有？」において、駆動ユニット速度計算・制御部４４は、ガイド操作部７に対して要求移動速度ｖ_refの入力の操作が有り、かつ、角度入力部５に対して目標角度の入力が有るかを判断する。これらのいずれか一方でも入力がない場合には、このステップＳ１０１で待機する。両方の入力が有った場合にはステップＳ１０２に進む。

ステップＳ１０２の「位置、要求移動速度、角度記憶」において、制御装置４の角度記憶部４２は、角度入力部５から目標角度を取得して記憶する。また、駆動ユニット２の位置・速度計算部から駆動ユニット２の位置を取得して記憶し、ガイド操作部７から要求移動速度ｖ_refを取得して記憶する。

ステップＳ１０３の「距離Ｌ_c移動？」において、駆動ユニット速度計算・制御部４４は、ステップＳ１０１でガイド操作部７に対して要求移動速度ｖ_refの入力の操作が有り、かつ、角度入力部５に対して目標角度の入力の操作が有ってから連続体ロボット１が距離Ｌ_c移動したか否かを判断する。移動した場合にはステップＳ１０１に戻る。移動していない場合にはステップＳ１０４に進む。

ステップＳ１０４の「追従可能速度計算／要求移動速度読み出し」において、駆動ユニット速度計算・制御部４４は、角度記憶部４２に記憶されている第２湾曲部１１２の遠位端の目標角度θ₂を読み出す。そして、数式（１８）、数式（１９）、数式（２２）、数式（２３）を用いて第１湾曲部１１１と第２湾曲部１１２の目標角度を計算し、計算した目標角度を用いて追従可能速度を計算する。また、駆動ユニット速度計算・制御部４４は、角度記憶部４２に記憶されている要求移動速度ｖ_refを読み出す。

ステップＳ１０５の「追従条件充足？」において、駆動ユニット速度計算・制御部４４は、ガイド操作部７から取得した要求移動速度ｖ_refが、数式（２９）の条件を充足するかどうかを判断する。すなわち、駆動ユニット速度計算・制御部４４は、要求移動速度ｖ_refが、計算したそれぞれの湾曲部についての追従可能速度のうちで最も小さい追従可能速度に係数αを乗じた値以下であるか否かを判断する。これにより、駆動ユニット速度計算・制御部４４は、駆動ユニット２を要求移動速度ｖ_refで前後進動作させた場合に、連続体ロボット１が目標経路Ｔから逸脱するか否かを判断する。数式（２９）の条件を充足する場合（Ｙｅｓ）にはステップＳ１０６に進み、充足しない場合（Ｎｏ）にはステップＳ１０７に進む。

ステップＳ１０６の「目標移動速度＝要求移動速度」において、駆動ユニット速度計算・制御部４４は、移動駆動部３３による駆動ユニット２の目標移動速度を、ガイド操作部７から取得した要求移動速度ｖ_refに設定する。一方、ステップＳ１０７の「目標移動速度＝追従可能速度」において、駆動ユニット速度計算・制御部４４は、駆動ユニット２の目標移動速度を、それぞれの湾曲部についての追従可能速度のうちで最も小さい追従可能速度に係数αを乗じた値に設定する。

ステップＳ１０８の「移動指令有？」においては、駆動ユニット速度計算・制御部４４は、前進動作の指令または後進動作の指令があるか否かを判断する。前進動作の指令と後進動作の指令のいずれもない場合にはステップＳ１０５に戻る。前進動作の指令または後進動作の指令がある場合には、ステップＳ１０９に進む。

ステップＳ１０９の「湾曲部駆動、駆動ユニット移動」において、角度制御部４１は、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂が計算した目標角度に一致するように、第１角度制御モータ２１１と第２角度制御モータ２１２を駆動する。すなわち、数式（７）と数式（１６）で規定される回転角度だけ、第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５を回転させる。また、駆動ユニット速度計算・制御部４４は、移動駆動部３２を制御し、ステップＳ１０６またはステップＳ１０７において設定した目標移動速度で駆動ユニット２を前進動作または後進動作させる。これにより、連続体ロボット１を、設定した目標移動速度で距離Ｌ_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を用いる構成によれば、駆動ユニット２の制動の遅れを防止できる。したがって、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂を目標角度に一致させ、第１湾曲部１１１と第２湾曲部１１２が目標経路Ｔから大きく逸脱することを抑制できる。また、駆動ユニット２の減速を、連続体ロボット１が目標経路Ｔから逸脱しないようにするための最小限に抑制できる。

（制御装置の処理）
次に、制御装置４が実行する処理の例について説明する。図１０と図１１は、制御装置４が実行する処理の例を示すフローチャートである。なお、図１０は処理全体を示し、図１１は図１０のＡ〜Ｂの間の処理を示す。

ステップＳ２０１の「目標角度入力有？」において、角度制御部４１は、目標角度の入力が有ったか否かを判断する。目標角度の入力がない場合（Ｎｏ）にはこのステップ２０１で待機する。目標角度の入力が有った場合（Ｙｅｓ）にはステップＳ２０２に進む。

ステップＳ２０２の「位置、目標角度記憶」において、角度記憶部４２は、駆動ユニット２の位置・速度計算部から連続体ロボット１の位置を取得して記憶し、角度入力部５から入力された目標角度を取得して記憶する。そしてステップＳ２０３に進む。

ステップＳ２０３の「距離Ｌ_c移動？」において、ブレーキ制御部３４は、ステップＳ２０１で角度入力部５に対して目標角度の入力が有ってから駆動ユニット２が距離Ｌ_c移動したか否かを判断する。移動した場合にはステップＳ２０１に戻る。移動していない場合にはステップＳ２０４とステップＳ２０６に進む。

ステップＳ２０４の「目標角度読み込み」において、角度制御部４１は、角度記憶部４２に記憶されている目標角度を読み出す。そしてステップＳ２０５に進む。

ステップＳ２０５の「湾曲部駆動」において、角度制御部４１は、数式（１８）と数式（１９）を用いて第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂を計算する。さらに、角度制御部４１は、数式（１６）と数式（１７）を用いて第１角度制御モータ２１１と第２角度制御モータ２１２の出力軸２１３，２１５の回転角度φ₁，φ₂を計算する。そして、角度制御部４１は、計算した回転角度φ₁，φ₂となるように、第１角度制御モータ２１１と第２角度制御モータ２１２を制御する。これにより、第１湾曲部１１１と第２湾曲部１１２が駆動され、第１湾曲部１１１と第２湾曲部１１２の遠位端角度θ₁，θ₂が目標角度に一致する。

ステップＳ２０６の「目標角度読み込み」において、ブレーキ制御部３４は、角度記憶部４２に記憶されている目標角度を読み出す。そしてステップＳ２０７に進む。

ステップＳ２０７の「追従可能速度計算」において、ブレーキ制御部３４は、読み出した目標角度を用いて、第１湾曲部１１１と第２湾曲部１１２のそれぞれについての追従可能速度ｖ_u1，ｖ_u2を計算する。そしてステップＳ２０８に進む。

ステップＳ２０８の「移動速度取得」において、ブレーキ制御部３４は、駆動ユニット２の位置・速度計算部から、連続体ロボット１（駆動ユニット２）の現在の移動速度を取得する。取得した現在の移動速度が、今制御周期における連続体ロボット１の移動速度である。そしてステップＳ２０９に進む。

ステップＳ２０９の「追従条件充足？」において、ブレーキ制御部３４は、ステップＳ２０９において取得した今制御周期の連続体ロボット１の移動速度から、次の制御周期の連続体ロボット１の推定移動速度ｖ_estを計算する。そして、ブレーキ制御部３４は、推定移動速度ｖ_estが数式（２７）と数式（２８）で計算される追従可能速度ｖ_u1，ｖ_u2の両方以下である否かを判断する。少なくとも一方より大きい場合（Ｎｏ）にはステップＳ２１０に進み、両方以下である場合（Ｙｅｓ）にはステップＳ２１１に進む。なお、数式（２９）の左辺を推定移動速度ｖ_estに置き換えた数式が成立するか否かを判断してもよい。この場合、成立しなければ（Ｎｏ）にはステップＳ２１０に進み、成立すれば（Ｙｅｓ）にはステップＳ２１１に進む。

ステップＳ２１０の「ブレーキ作動」において、ブレーキ制御部３４は、ブレーキ部３４を作動させ、駆動ユニット２を制動して連続体ロボット１を減速させる。そしてステップＳ２１２に進む。なお、現在すでにブレーキ部３４が作動している場合には、ブレーキ部３４の作動を継続する。

ステップＳ２１１の「ブレーキ解除」において、ブレーキ制御部３４は、ブレーキ部３４の作動を解除する。そしてステップＳ２１２に進む。なお、ブレーキ部３４が作動していない場合には、ブレーキ部３４が作動していない状態を継続する。

ステップＳ２１２の「制御周期経過？」においては、ブレーキ制御部３４は、１制御周期が経過したか否かを終了したかを判断する。１制御周期が経過していない場合には、今制御周期が終了していないとして、ステップＳ２１０またはステップＳ２１１の処理を継続する。１制御周期が経過した場合には、ステップＳ２１３に進む。

ステップＳ２１３の「距離Ｌ_c移動？」において、ブレーキ制御部３４は、ステップＳ２０１で角度入力部５に対して目標角度の入力が有ってから連続体ロボット１が距離Ｌ_c移動したか否かを判断する。移動していない場合にはステップＳ２２０８に戻り、次の制御周期を開始する。移動した場合にはステップＳ２０１に戻る（図１０参照）。

このように、使用者による連続体ロボット１の移動速度が、連続体ロボット１が目標経路Ｔから逸脱しない条件を充足する場合には、ブレーキ制御部３４はブレーキ部３４による駆動ユニット２の制動を行わない。この場合には、使用者が所望の移動速度（すなわち、要求移動速度）で連続体ロボット１を前後進動作させることができる。そして、この場合には、使用者が所望の速度で連続体ロボット１を前後進動作させても、連続体ロボット１は目標経路Ｔから逸脱しない。一方、使用者による連続体ロボット１（駆動ユニット２）の前後進動作の移動速度が、連続体ロボット１が目標経路Ｔから逸脱しない条件を充足しない場合には、ブレーキ制御部３４はブレーキ部３４を制御して駆動ユニット２を制動させる。これにより、連続体ロボット１を減速させる。このためこの場合には、連続体ロボット１の前後進動作の移動速度を、連続体ロボット１が目標経路Ｔから逸脱しない速度に留めることができる。そして、制御装置４は、このような処理を所定の制御周期で繰り返し実行する。このような構成によれば、連続体ロボット１を前後進動作させる際に、連続体ロボット１が目標経路Ｔから逸脱しないようにできる。さらに、前後進動作の移動速度の低下を最小限にとどめることができる。このように、第２の実施形態によれば、第１の実施形態と同様の効果を奏することができる。

＜他の実施形態＞
前記各実施形態では、連続体ロボット１が第１湾曲部１１１と第２湾曲部１１２の２つの湾曲部を有する構成を例に示した。しかしながら、本発明は、３つ以上の湾曲部を有する連続体ロボット１の制御にも適用できる。湾曲部の数をＡとすると、遠位端湾曲部の位置ｓ_Aが、ｎＬ_c≦ｓ_A≦（ｎ＋１）Ｌ_cの範囲にあるとき、近位端側からｉ （１≦ｉ≦Ａ）番目の湾曲部の位置は、（ｎ−Ａ＋ｉ）Ｌ_c≦ｓ_A≦（ｎ−Ａ＋ｉ＋１）Ｌ_cの範囲にある。したがって、次の数式（３０）と数式（３１）を用いて、ｉ番目の湾曲部の角度目標値の計算と追従可能速度ｖ_uiの計算を行えばよい。なお、数式（３０）と数式（３１）は、それぞれ数式（１８）と数式（２８）のｎをｎ−Ａ＋ｉで置き換えた式である。

また、本発明は各湾曲部の長さがそれぞれ異なる連続体ロボット１に対しても適用可能である。さらに、前記各実施形態では、連続体ロボット１が距離Ｌ_c進むごとに遠位端湾曲部の目標角度ψの入力を要求したが、目標角度の入力を行うまでに連続体ロボット１が進む距離は湾曲部の長さと異なっていてもよい。遠位端湾曲部からｉ番目の湾曲部までの長さをＬ_iとすると、遠位端湾曲部の位置がｓ_Aのとき、ｉ番目の湾曲部の位置はｓ_A−Ｌ_iとなる。また、遠位端湾曲部が距離Ｌ_d進むごとに目標角度ψの入力作業を行うとすると、角度記憶部４２はＬ_dの整数倍ごとの目標角度ψを記憶していく。このとき、角度制御部４１はｎ_iＬ_d≦ｓ_A−Ｌ_i≦（ｎ_i＋１）Ｌ_dを満たす整数ｎ_iを計算し、角度記憶部４２から目標角度ψ_ni+1とψ_niを読み込む。そして、これらの目標角度を数式（２０）と数式（２１）に代入して遠位端角度θ₁，θ₂を計算し、移動駆動部３２の速度制御やブレーキ部３４の制動の判断を行う。

また、前記各実施形態では、角度入力部５は、第２湾曲部１１２が現在位置より所定の距離Ｌ_c前進したときの目標角度ψの入力に用いられる構成を示したが、このような構成に限定されない。例えば、操作性を向上させるために一般的な内視鏡と同様に、現在位置における目標角度を入力するために用いられる構成であってもよい。この場合、連続体ロボットシステムにおいて駆動ユニット２を前進させるとき、角度制御部４１は、数式（１８）の角度ψ_n+1にψ_nを代入した次の数式（３２）を用いて、第２湾曲部１１２の遠位端角度θ₂を制御する。

この数式（３２）より、現在位置からの移動距離ｌ（０≦ｌ≦Ｌ_c）において第２湾曲部１１２の目標角度は一定となるため、第２角度制御モータ２１２を駆動しなくてもよい。このため、駆動ユニット速度計算・制御部４４は、第１角度制御モータ２１１に関する数式（２７）のみを用いて移動駆動部３２の制御を行う。また、ブレーキ制御部３４は、第１角度制御モータ２１１に関する数式（２７）のみを用いてブレーキ部３４の制御を行う。なお、現在位置での目標角度ψを入力する場合においても、前進時に入力した目標角度が角度記憶部４２に記憶されるため、後進時には前記各実施形態と同様に、数式（２７）と数式（２８）の両方を用いて、移動駆動部３２やブレーキ部３４を制御する。

また、前記各実施形態では、連続体ロボット１が距離Ｌ_c進むごとに、使用者が目標角度ψを入力（設定）し、制御装置４がこの目標角度ψを取得したが、このような構成に限定されない。例えば、前進動作を行う前に連続体ロボット１が通過すべき目標経路Ｔが既知である場合には、距離Ｌ_c進むごとの入力（設定）や取得を省略できる。例えば、目標経路Ｔから目標角度ψを計算する方法としては、まず目標経路Ｔをそれぞれの長さがＬ_cである複数の区間に分割し、各区間の端点おける経路の接線を計算し、各端点における接線とＸ₁軸のなす角度を目標角度ψとする方法が適用できる。この場合、角度記憶部４２に目標経路Ｔをあらかじめ記憶させておき、駆動ユニット速度計算・制御部４４やブレーキ制御部３４が前記計算によって目標角度ψを計算するという構成が適用できる。また、使用者が目標経路Ｔの各区間の端点の目標角度を角度記憶手段に記憶させる構成であってもよい。このような構成によれば、使用者は前後進動作中に目標角度ψの入力を行わなくてもよくなる。

さらに、本発明は、複数の姿勢変更部（角度制御モータ）を用いて１つの湾曲部を立体的に駆動する連続体ロボットに対しても適用可能である。例えば、連続体ロボットシステムが軟性内視鏡に適用される場合、挿抜経路である食道や気管支や肺や大腸など人体の器官は立体的に湾曲している部分を有している。このため、これらの器官に沿って連続体ロボット１を挿入・抜去するためには、複数の姿勢変更部（角度制御モータ）を用いて各湾曲部を立体的に駆動しながら先頭追従動作を行うことが望ましい。この場合、駆動ユニット速度計算・制御部４４は、全ての姿勢変更部について、数式（２９）を充足するか否かを判断し、判断結果に応じて目標移動速度を設定する。また、ブレーキ制御部３４は、全ての姿勢変更部について目標角速度が最大値以下となるようにブレーキ部３４を制御する。例えば、前記第１の実施形態と同様に、駆動ユニット２の速度から湾曲部が目標経路Ｔから逸脱しないという条件を充足するためのモータの角速度を計算する。そして、少なくとも一つのモータについて、要求される角速度が最大角速度を超過するときには、ブレーキ部３４を制御して駆動ユニット２を制動する。

また、本発明は、モータの最大回転速度に加えて、モータの加速度の上限によって生じる湾曲部の応答の遅れを考慮して連続体ロボット１の移動速度を制御することも可能である。連続体ロボット１の最大速度に関する条件を表す数式（３１）を時間で微分すると、次の数式（３３）が得られる。数式（３３）の左辺は連続体ロボット１（駆動ユニット２）の加速度であり、左辺のａ_maxはモータの最大角加速度である。

駆動ユニット速度計算・制御部４４は、センサ２４から取得した連続体ロボット１の移動速度ｖから連続体ロボット１の加速度を計算する。そして、駆動ユニット速度計算・制御部４４は、計算した加速度が数式（３３）を充足しない場合には、連続体ロボット１の目標移動速度を最大速度ｖ_maxより低い値に設定する。これにより、モータを最大角加速度以下で駆動しながら先頭追従制御を行うことが可能となる。また、計算した加速度が数式（３３）を充足しない場合には、ブレーキ制御部３４は、ブレーキ部３４を作動させることで、モータを最大角加速度以下で駆動しながら先頭追従制御を行うことが可能となる。

（その他の適用例）
前記各実施形態では、連続体ロボットシステムが軟性内視鏡に適用される例を示したが、本発明に係る例御システムの適用対象は軟性内視鏡に限定されない。例えば、配管内部や狭隘な空間に挿入して作業を行う産業用の連続体ロボットの制御にも適用できる。この場合、連続体ロボットの遠位端に設けられているカメラを、適用対象に応じて適宜変更すればよい。例えば、カメラのほかに、各種工具や機器が設けられる構成が適用できる。

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

さらに、前記実施形態では、ＣＰＵとＲＯＭなどの記憶媒体とＲＡＭとを有するコンピュータがプログラムを実行することによりコンピュータが制御装置の各部として機能する構成を示したが、このような構成に限定されない。例えば、前記制御装置の各部が、それぞれ個別のハードウェアによって構築される構成であってもよい。

本発明は、連続体ロボットシステム、連続体ロボットシステムの作動方法に好適な技術である。そして、本発明によれば、連続体ロボットを前進や後進させる際に、連続体ロボットが目標経路から逸脱しないようにできる。

Claims (12)

1. 長尺方向に直列に設けられた、それぞれが湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
   前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、
   前記複数の湾曲部のそれぞれを湾曲させることによりそれぞれの姿勢を変更する姿勢変更部と、
   を有する連続体ロボットシステムの制御装置であって、
   前記制御装置は、
   前記移動駆動部を制御する移動駆動制御手段と、
   前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
   を有し、
   前記移動駆動制御手段は、前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下にすることを特徴とする連続体ロボットシステムの制御装置。
2. 前記移動駆動制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記移動速度を、計算した前記最大値のうちで最も小さい値にすることを特徴とする請求項１に記載の連続体ロボットシステムの制御装置。
3. 前記移動駆動制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記移動速度を、計算した前記最大値のうちで最も小さい値に１以下の値の係数を乗じた値にすることを特徴とする請求項２に記載の連続体ロボットシステムの制御装置。
4. 前記移動駆動制御手段は、前記移動速度を、閾値以上の速度にすることを制御することを特徴とする請求項１から３のいずれか１項に記載の連続体ロボットシステムの制御装置。
5. 長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
   前記連続体ロボットを前記長尺方向の移動速度を減速させるブレーキ部と、
   前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
   を有する連続体ロボットシステムの制御装置であって、
   前記制御装置は、
   前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
   前記ブレーキ部を制御するブレーキ制御手段と、
   を有し、
   前記ブレーキ制御手段は、前記連続体ロボットの前記長尺方向の移動速度が前記最大値を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させることを特徴とする連続体ロボットシステムの制御装置。
6. 前記ブレーキ制御手段は、前記複数の湾曲部のそれぞれについて前記最大値を計算し、前記連続体ロボットの前記長尺方向の移動速度が前記最大値のうちで最も小さい移動速度を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させることを特徴とする請求項５に記載の連続体ロボットシステムの制御装置。
7. 長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
   前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
   を有する連続体ロボットシステムの制御装置であって、
   前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算手段と、
   前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下に制限する移動速度制限手段と、
   を有することを特徴とする連続体ロボットシステムの制御装置。
8. 前記姿勢変更部は、前記複数の湾曲部の長尺方向の一方の端部に設けられており、
   前記複数の湾曲部のそれぞれの前記姿勢は、前記複数の湾曲部のそれぞれについて、中心線に直角であり前記一方の端部とは反対側の端部を通過する直線と、移動方向に直角であり前記複数の湾曲部のうちの前記姿勢変更部に最も近い湾曲部の前記姿勢変更部に近い側の端部を通過する直線とがなす角度であることを特徴とする請求項１から７のいずれか１項に記載の連続体ロボットシステムの制御装置。
9. 長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
   前記連続体ロボットを前記長尺方向に移動させる移動駆動部と、
   前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
   を有する連続体ロボットシステムの作動方法であって、
   前記移動駆動部が前記連続体ロボットを前記長尺方向に所定の距離の移動を完了させるまでに前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
   前記移動駆動部による前記連続体ロボットの前記長尺方向の移動速度を前記最大値以下に設定するステップと、
   を有することを特徴とする連続体ロボットシステムの作動方法。
10. 長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
    前記連続体ロボットを前記長尺方向の移動速度を減速させるブレーキ部と、
    前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
    を有する連続体ロボットシステムの作動方法であって、
    前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
    前記連続体ロボットの前記長尺方向の移動速度が前記最大値を超える場合には、前記ブレーキ部により前記連続体ロボットの前記長尺方向の移動速度を減速させるステップと、
    を有することを特徴とする連続体ロボットシステムの作動方法。
11. 長尺方向に直列に設けられた湾曲可能な複数の湾曲部を有し、前記複数の湾曲部の長尺方向に移動可能な連続体ロボットと、
    前記複数の湾曲部のそれぞれの姿勢を変更する姿勢変更部と、
    を有する連続体ロボットシステムの作動方法であって、
    前記連続体ロボットが前記長尺方向に所定の距離の移動を完了するまでに、前記姿勢変更部が前記複数の湾曲部の姿勢を目標姿勢に一致させることができる前記連続体ロボットの前記長尺方向の移動速度の最大値を計算する速度計算ステップと、
    前記連続体ロボットの前記長尺方向の移動速度を、前記最大値以下に制限する移動速度制限ステップと、
    を有することを特徴とする連続体ロボットシステムの作動方法。
12. コンピュータを、請求項１から８のいずれか１項に記載の制御装置として機能させることを特徴とするプログラム。

Images