JP7781562B2 - 連続体ロボットの制御システム及びその制御方法 - Google Patents

Description

本発明は、連続体ロボットの制御システム及びその制御方法に関するものである。

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

この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。この連続体ロボットの駆動方法としては、腱駆動によるもの、押し引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。

特許文献１には、駆動用のワイヤに発生する負荷量を検出し、負荷量が予め設定された範囲となるように、駆動部を制御するマニピュレータの制御方法が記載されている。これにより、特許文献１では、マニピュレータを狭小空間に挿通する際に、その経路に沿うための湾曲操作をすることなく挿入が可能とし、挿通動作を容易にしている。ここで、駆動部が負荷量に応じて受動的に動作することを「バックドライバビリティがある」というが、特許文献１では、クラッチなどの機構ではなく、ワイヤに発生する負荷量の検出と制御アルゴリズムを用いており、駆動部の大型化を防いでいる。具体的に、特許文献１に記載のマニピュレータは、筒状部とよばれる連続体と、その先端の複数の関節構造からなり、関節に連結されるワイヤを基端部筐体に設けられたモータにより牽引することで動作し、また、複数の節輪と回動関節で連結される連続体に対して適用可能としている。

特許第６１６９０４９号公報

特許文献１に記載のマニピュレータでは、「挿通動作モード」と「動作制御モード」とがある。「挿通動作モード」では、１）関節に周期的な往復運動を行わせる動作指令を与え、ワイヤに発生する負荷量に応じて回動量を低減するように制御する方法と、２）ワイヤに発生する負荷量を検知し、それを低減するようにモータを駆動する方法が開示されている。このため、「挿通動作モード」は、「動作制御モード」よりもバックドライバビリティが高い動作制御となっている。しかしながら、特許文献１に記載のマニピュレータでは、「挿通動作モード」と「動作制御モード」との切り替えを操作者が行っているため、操作者の負担が増大し、また、適切な切り替えを行うという観点では不十分であった。

本発明は、このような問題点に鑑みてなされたもので、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行える仕組みを提供することを目的とする。

本発明の連続体ロボットの制御システムは、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、を有し、前記第２の駆動制御部による駆動制御は、前記ワイヤの変位に関する目標値と検出値との間の差異を補償する制御と、前記ワイヤに印加される力に関する目標値と検出値との間の差異を補償する制御を含む。
また、本発明の連続体ロボットの制御システムは、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度を算出するための逆運動学演算を行う逆運動学演算部と、を有し、前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記モード信号として前記第１の駆動制御部を示す信号が入力された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う。
また、本発明の連続体ロボットの制御システムは、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度を算出するための逆運動学演算を行う逆運動学演算部と、を有し、前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記ワイヤに印加された力の検出結果として所定範囲内の力が検出された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う。
また、本発明の連続体ロボットの制御システムは、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、前記運動学演算部は、前記湾曲可能部の湾曲角度と基準軸に対する前記湾曲可能部の旋回角度とから前記ワイヤの駆動量を算出するための運動学演算を行うものであり、前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度および前記湾曲可能部の旋回角度を算出するための逆運動学演算を行う逆運動学演算部と、を有し、前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記モード信号として前記第１の駆動制御部を示す信号が入力された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合であって前記運動学演算部に入力された前記湾曲可能部の旋回角度と前記逆運動学演算部から出力された前記湾曲可能部の旋回角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う。
また、本発明の連続体ロボットの制御システムは、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、前記運動学演算部は、前記湾曲可能部の湾曲角度と基準軸に対する前記湾曲可能部の旋回角度とから前記ワイヤの駆動量を算出するための運動学演算を行うものであり、前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度および前記湾曲可能部の旋回角度を算出するための逆運動学演算を行う逆運動学演算部と、を有し、前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記ワイヤに印加された力の検出結果として所定範囲内の力が検出された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合であって前記運動学演算部に入力された前記湾曲可能部の旋回角度と前記逆運動学演算部から出力された前記湾曲可能部の旋回角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う。

本発明によれば、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行うことができる。

本発明の第１の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 本発明の第１の実施形態における連続体ロボットの動的モデルの一例を示す図である。 本発明の第１の実施形態における連続体ロボットの動的モデル（拡大系）のボード線図である。 図１に示す高バックドライバビリティ駆動制御部（Ｋ_BKD）の機能構成の一例を示す図である。 図５に示す力制御部（Ｋ_F）のボード線図と、図５に示す開ループ伝達関数のボード線図である。 図５に示す位置制御部（Ｋ_SV）のボード線図と、図５に示す開ループ伝達関数のボード線図である。 図１に示す低バックドライバビリティ駆動制御部（Ｋ_POS）のボード線図と、閉ループ系との開ループ伝達関数のボード線図である。 本発明の第１の実施形態におけるシミュレーションの模式図である。 本発明の第１の実施形態におけるシミュレーションの応答特性を示す図である。 本発明の第１の実施形態におけるシミュレーションの応答特性を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第２の実施形態におけるシミュレーションの応答特性を示す図である。 本発明の第３の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 図１４に示す第ｎの湾曲可能部の部分を抽出した図である。 図１４に示す連続体ロボットの先頭追従制御を説明するための図である。 図１６に示す連続体ロボットの先頭追従制御の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第３の実施形態を示し、図１６に示す連続体ロボットの先頭追従制御の一例を示す図である。 本発明の第３の実施形態におけるシミュレーションの応答特性を示す図である。 本発明の第３の実施形態におけるシミュレーションの応答特性を示す図である。 本発明の第４の実施形態を示し、図１６に示す連続体ロボットの先頭追従制御の一例を示す図である。 本発明の第５の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 本発明の第５の実施形態に係る連続体ロボットのワイヤの配置の一例を示す図である。 本発明の第５の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。

以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。

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

第１の実施形態では、ワイヤの張力検出が可能なワイヤの駆動部を含む、連続体ロボットの動的モデルを導出する。そして、バックドライバビリティを切り替え可能な制御系を設計する。

図１は、本発明の第１の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図１に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－１」として記載する。

連続体ロボットの制御システム１０－１は、図１に示すように、連続体ロボット（Ｐ_n）１００、入力部２１１，２１２、切替制御部（Ｈ）２２０、運動学演算部（Kinematics）２３０、遮断部２４１，２４２を有して構成されている。さらに、連続体ロボットの制御システム１０－１は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、選択部２６０、逆運動学演算部（ＩＫ）２７０を有して構成されている。

連続体ロボット（Ｐ_n）１００は、ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、ワイヤを駆動する駆動部を備えて構成されている。入力部２１１は、選択する駆動制御部を示すモード信号を切替制御部（Ｈ）２２０に対して入力する構成部である。入力部２１２は、基準軸に対する湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θを運動学演算部（Kinematics）２３０等に対して入力する構成部である。運動学演算部（Kinematics）２３０は、基準軸に対する前記湾曲可能部の湾曲角度（目標指令値ｒｅｆ_θ）からワイヤの駆動量（ワイヤ把持機構の目標変位ｒｅｆ_z）を算出するための運動学演算を行う。低バックドライバビリティ駆動制御部（Ｋ_POS）２５１は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２よりも、バックドライバビリティが低い駆動制御部である。この低バックドライバビリティ駆動制御部（Ｋ_POS）２５１は、上述した運動学演算の結果に基づいて駆動部によるワイヤの駆動量を制御する第１の駆動制御部である。高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１よりも、バックドライバビリティが高い駆動制御部である。即ち、この高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、湾曲可能部が外力を受けたときに低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御の場合よりも駆動部が逆方向に駆動されやすい第２の駆動制御部である。この高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２も、上述した運動学演算の結果に基づいて駆動部によるワイヤの駆動量を制御する。遮断部２４１は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の前段に設けられている。遮断部２４２は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の前段に設けられている。選択部２６０は、切替制御部（Ｈ）２２０の制御（信号Ｈ_OUT）に基づいて、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の出力（ｕ_POS）または高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の出力（ｕ_BKD）を選択する。そして、選択部２６０は、選択した出力を連続体ロボット（Ｐ_n）１００に出力する。逆運動学演算部（ＩＫ）２７０は、運動学演算部（Kinematics）２３０による運動学演算とは逆の演算であってワイヤの駆動量から湾曲可能部の湾曲角度を算出するための逆運動学演算を行う。切替制御部（Ｈ）２２０は、選択する駆動制御部を示すモード信号またはワイヤに印加された力の検出結果に応じて、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２とを切り替える制御を行う。

具体的に、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を有効にするときは、信号ｈ_POSをオンにして信号ｈ_BKDをオフにする。これにより、遮断部２４２によって高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への制御誤差入力が遮断される。一方、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を有効にするときは、信号ｈ_POSをオフにして信号ｈ_BKDをオンにする。これにより、遮断部２４１によって低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への制御誤差入力が遮断される。

まず、切替制御部（Ｈ）２２０による高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えについて説明する。高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、入力部２１１から切替制御部（Ｈ）２２０にモード信号として低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示す信号が入力された場合である。この場合、切替制御部（Ｈ）２２０は、運動学演算部（Kinematics）２３０に入力された湾曲可能部の湾曲角度と逆運動学演算部（ＩＫ）２７０から出力された湾曲可能部の湾曲角度との差分θ_errが閾値以下である場合に、当該切り替えを行う。これにより、当該切り替え時の目標値への急峻な追従が抑えられ、連続体ロボット自体や脆弱な対象物の損傷を防ぐことが実現できる。

続いて、切替制御部（Ｈ）２２０による低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えについて説明する。低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えでは、切替制御部（Ｈ）２２０は、差分θ_errの値に関わらず、当該切り替えを行う。具体的に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御が行われてる際に、入力部２１１から切替制御部（Ｈ）２２０にモード信号として高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を示す信号が入力された場合である。これにより、必要に応じてバックドライバビリティを可変にすることが可能な連続体ロボットが実現できる。

以下に、連続体ロボット１００の動的モデルＰ_nの導出と制御アルゴリズムについて詳細に記述する。

１．１）モデリング
図２は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。以下、図２に示す連続体ロボット１００を「連続体ロボット１００－１」として記載する。

連続体ロボット１００－１は、湾曲可能部１１０の遠位端１６０における固定部１２１及び固定部１２２に、それぞれ、ワイヤ１１１及びワイヤ１１２が接続されている。また、ワイヤ１１１及びワイヤ１１２の近位端は、ロボット基台１４０の中で、それぞれ、ワイヤ把持パイプ１３１及びワイヤ把持パイプ１３２に接続されている。ロボット基台１４０には、ワイヤ把持パイプ１３１を支持するワイヤ把持機構１７１、ワイヤ把持パイプ１３２を支持するワイヤ把持機構１７２、及び、駆動部に相当するアクチュエータ１８０が配置されている。この際、ワイヤ把持機構１７２は、ロボット基台１４０に固定されている。また、ワイヤ把持機構１７１は、駆動部であるアクチュエータ１８０にワイヤ把持機構基台（不図示）を介して接続され、上下に移動可能である。ワイヤ把持パイプ１３１は、ワイヤ把持機構１７１に支持可能に接続され、アクチュエータ１８０で押引きすることにより湾曲可能部１１０の姿勢が制御される。また、連続体ロボット１００－１は、ワイヤ１１１及びワイヤ１１２を案内するための部材であるワイヤガイド１６１～１６４を有して構成されている。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１６１～１６４は、ワイヤ１１２に、固定部１５０～１５３において固定される。さらに、ワイヤ１１１及びワイヤ１１２の間隔と、ワイヤ把持機構１７１及びワイヤ把持機構１７２との間隔が異なってもよい。そのときは、径変換部１９０をロボット基台１４０に接続する。

本実施形態では、ワイヤ１１１及びワイヤ１１２と、ワイヤガイド１６１～１６４からなる機構を連続体部分である湾曲可能部１１０とよぶ。そして、本実施形態では、図２に示すように、湾曲可能部１１０の遠位端１６０においてｚ軸と平行な基準軸１０１に対する湾曲可能部１１０の湾曲角度をθとして定めている。また、アクチュエータ１８０には、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構１７１（或いはアクチュエータ１８０）は、ワイヤ１１１の張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構１７１とアクチュエータ１８０との間に、上述したワイヤ把持機構基台（不図示）を設け、このワイヤ把持機構基台をアクチュエータ１８０に接続し、ワイヤ把持機構基台とワイヤ把持機構１７１との間をばねで接続する。このとき、ワイヤ把持機構１７１がｚ軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤ１１１の張力が検出可能となる。

図３は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰ_nの一例を示す図である。具体的に、図３（ａ）は、連続体部分である湾曲可能部１１０の動的モデルの一例を示し、また、図３（ｂ）は、アクチュエータ１８０に含まれる回転モータ及び回転直動変換機構とワイヤ把持機構１７１等からなる動的モデルの一例を示す。

１．１．１）連続体部分である湾曲可能部１１０の動的モデル
図３（ａ）に示す連続体部分である湾曲可能部１１０の動的モデルにおける符号の定義を以下に記載する。
θ：基準軸１０１に対する湾曲可能部１１０の湾曲角度
ρ：湾曲可能部１１０の曲率半径
ｌ：湾曲可能部１１０の長さ
ｒ₁：湾曲可能部１１０の中心軸からワイヤまでの距離
ｌ_p：ワイヤの駆動量
ｍ_s：湾曲可能部１１０の質量
ｋ_b：湾曲可能部１１０の湾曲角度に対するばね係数
ｚ_w：ワイヤ等価質量の変位
ｚ_p：ワイヤ把持パイプの変位
ｍ_w：ワイヤの質量
ｍ_p：ワイヤ把持パイプの質量
ｋ_w，ｋ_w3：ワイヤのばね係数
ｃ_w，ｃ_w3：ワイヤの減衰係数
ｃ_w2：ワイヤと径変換部及びワイヤガイドとの摩擦による減衰係数

続いて、連続体ロボット１００－１の運動方程式を導出する。本実施形態では、以下の仮定をおく。
［１］２次元平面内での運動のみを考慮する。
［２］湾曲可能部１１０の曲率は一定とし、ばね係数は均一であるものとする。
［３］ワイヤは集中質点系として近似し、長手方向の変形による反力が湾曲可能部１１０の先端に作用するものとする。ワイヤの横振動及び横方向の変形は考慮しない。
［４］ワイヤとワイヤガイドとの間、及び、ワイヤと径変換部との間の摩擦は、クーロン摩擦等の非線形性摩擦を含めて粘性減衰として減衰係数ｃ_w2に押し込める。

まず、湾曲可能部１１０の運動エネルギーを求める。湾曲可能部１１０の中心軸上に変位ｘ_g及びｚ_gをとると、それぞれ、以下の（１）式及び（２）式となる。

また、以下の（３）式
であるから、変位ｘ_g及びｚ_gは、それぞれ、以下の（４）式及び（５）式となる。
これを用いて、湾曲可能部１１０の運動エネルギーＴ_aは、以下の（６）式となる。

また、湾曲可能部１１０の位置エネルギーＵ_aは、以下の（７）式となる。

次に、ワイヤ及びワイヤ把持パイプの運動エネルギーＴ_wと位置エネルギーＵ_wは、ワイヤ駆動量がｌ_p＝ｒ₁θであるから、それぞれ、以下の（８）式及び（９）式となる。
そして、運動方程式をラグランジュの方程式を示す以下の（１０）式
より求めると、以下の（１１）式に示す非線形微分方程式が得られる。以下の（１１）式において、Φは、Ｍ_c，Ｃ_c，Ｋ_cに入れることのできない非線形項とする。
また、（１１）式のＭ_c22及びΦ₃は、以下の（１２）式及び（１３）式となる。

しかしながら、湾曲可能部１１０の湾曲角度が０度の近傍では、（１２）式及び（１３）式は不定形となり、値を求めることができない。そこで、大変形を考慮しない０度近傍線形化モデルを求める。（４）式及び（５）式より、以下の（１４）式となる。
そして、湾曲可能部１１０の０度近傍での運動エネルギーＴ_a0は、以下の（１５）式となる。
そして、Ｔ＝Ｔ_a0＋Ｔ_wとおいて、ラグランジュの方程式より運動方程式を求めると、以下の（１６）式となる。

１．１．２）回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデル
図３（ｂ）は、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの一例を示す図である。ワイヤ把持機構は、ワイヤの張力を検出する機能を持ち、ワイヤ把持機構とアクチュエータ１８０との間にワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータ１８０に接続し、ワイヤ把持機構基台とワイヤ把持機構との間をばねで接続する。そのばねの変位を検出することにより張力検出を行う。図３（ｂ）に示す動的モデルにおける符号の定義を以下に記載する。
Ｊ_m：モータのイナーシャ
θ_m：モータ回転角度
Ｔ_m：モータトルク指令値
ｃ_m：モータ軸の減衰係数
ｋ_g，ｃ_g：カップリングのばね係数，減衰係数
Ｊ_n：駆動軸のイナーシャ
θ_n：駆動軸の回転角度
ｃ_n：駆動軸の減衰係数
ｐ：駆動軸のねじピッチ
Ｒ：直動回転変換機構の変換係数であって等価的増速比
ｍ_t1：ワイヤ把持機構基台の質量
ｋ_t1：駆動軸のｚ方向のばね係数
ｃ_t1：ワイヤ把持機構基台のリニアスライダの減衰係数
ｚ_t1：ワイヤ把持機構基台の変位
ｍ_t2：ワイヤ把持機構の質量
ｋ_t2，ｃ_t2：張力検出機構のばね係数
ｚ_t2：ワイヤ把持機構の変位

そして、運動方程式は、以下の（１７）式～（２０）式のようになる。
ここでは、Ｒ＝p／２πとおいている。ｑ_l＝［θ_m，θ_n，ｚ_t1，ｚ_t2］^Tとして行列表記すると、以下の（２１）式となる。

１．１．３）拡大系の構成
図３（ｃ）に示すように、図３（ａ）に示す湾曲可能部１１０の動的モデルと、図３（ｂ）に示す回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルとを結合した拡大系を、連続体ロボット１００の動的モデルＰ_nとする。ワイヤ把持機構の変位ｚ_t2とワイヤ把持パイプの変位ｚ_bを同一とすることで、（１１）式と（２１）式によって拡大系を構成すると、運動方程式は、以下の（２２）式のようになる。ここで、ｑ_g＝［ｑ_l ^T，ｚ_w，θ］^Tとしている。

次に、状態方程式を求める。湾曲可能部１１０の湾曲角度が０度の近傍にないときは、非線形項Φ_gを行列Ｋ_gに押し込めて線形化を行う、以下の（２３）式の拡張線形化法を適用する。
これにより、Φ₃は、Ｋ_g（６，６）の要素となる。そして、状態方程式は、以下の（２４）式及び（２５）式となる。

本実施形態では、ワイヤ把持機構基台の変位ｚ_t1及びワイヤ把持機構の変位ｚ_t2が測定可能とし、ばねｋ_t2の発生する力がワイヤ把持機構に及ぼす力（以下、発生力）Ｆ、及びワイヤ把持機構の変位ｚ_t2を観測量とする。ここで、発生力Ｆは、以下の（２６）式となる。

Ｆ ＝ －ｋ_t2（z_t2 － z_t1） (26)

このとき、観測量ｙ_gは、以下の（２７）式に示す出力方程式で表される。

湾曲可能部１１０の湾曲角度が０度の近傍にあるときは、湾曲可能部１１０の０度近傍での運動方程式を示す（１６）式と、回転モータ及び回転直動変換機構とワイヤ把持機構からなる動的モデルの運動方程式を示す（２１）式により、同様に拡大系を構成する。これは線形モデルであるので、そのまま状態方程式に変換すればよい。

図４は、本発明の第１の実施形態における連続体ロボット１００の動的モデルＰ_n（拡大系）のボード線図である。図４では、モータトルク指令値Ｔ_mから発生力Ｆまでの伝達関数を実線で示し、ワイヤ把持機構の変位ｚ_t2までを破線で示し、湾曲可能部１１０の湾曲角度θまでを一点鎖線で示している。高周波数域には、ワイヤ張力検出機構のばね、ワイヤ、回転直動変換機構による高次振動が現れている。

１．２）制御系設計
１．２．１）バックドライバビリティ切り替え制御系
本実施形態は、図１に示すように、位置決め性能が高くバックドライバビリティの低い低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と、バックドライバビリティの高い高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を有するシステムである。ここで、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えは、モード信号に従って自動的に可能とする。しかしながら、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えは、モード信号に従って自動的に可能とするものではない。当該切り替えにおける更なる条件として、運動学演算部（Kinematics）２３０に入力された湾曲可能部の湾曲角度と逆運動学演算部（ＩＫ）２７０から出力された湾曲可能部の湾曲角度との差分θ_errが閾値以下である場合を条件とする。高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えにおいて、上述した更なる条件を課した理由は、次の通りである。高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２では、湾曲角度の目標指令値ｒｅｆ_θと逆運動学演算部（ＩＫ）２７０から出力される湾曲可能部の湾曲角度との誤差が大きくなる。この状態から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に無条件で切り替われば、連続体ロボット（Ｐ_n）１００は目標値に追従するために急峻に動作し、連続体ロボット自体や周辺に損傷を与える恐れがあるからである。

以下、図１～図２を用いた説明を行う。
連続体部分である湾曲可能部１１０の曲率を一定と仮定すると、ワイヤの駆動量ｌ_pと湾曲角度θとの関係は、以下の（２８）式で与えられる。

ｌ_p ＝ ｒ₁・θ (28)

ここで、ワイヤの伸縮量は微小と考えて運動学演算の導出に考慮しないとすると、湾曲可能部１１０の湾曲角度の目標指令値ｒｅｆ_θと、ワイヤ把持機構の目標変位ｒｅｆ_zから運動学演算部（Kinematics）２３０において、以下の（２９）式が求められる。

ｒｅｆ_z ＝ ｒ₁・ｒｅｆ_θ (29)

また、逆運動学演算部（ＩＫ）２７０では、ワイヤ把持機構の変位z_t2から湾曲可能部１１０の湾曲角度θ_IKを、以下の（３０）式により求める。
そして、この逆運動学による湾曲可能部１１０の湾曲角度θ_IKと湾曲可能部１１０の湾曲角度の目標指令値ｒｅｆ_θとの差分θ_errは、以下の（３１）式となる。

θ_err ＝ ｒｅｆ_θ－θ_IK (31)

この差分θ_errが、切替制御部（Ｈ）２２０に入力される。切替制御部（Ｈ）２２０は、駆動制御部を切り替えるために、信号ｈ_OUT、信号ｈ_POS、信号ｈ_BKDを出力する。信号ｈ_OUTは、選択部２６０に入力される。選択部２６０は、信号ｈ_OUTに従って、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の出力（ｕ_POS）または高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の出力（ｕ_BKD）を選択して、連続体ロボット（Ｐ_n）１００に出力する。

さらに、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を有効にするときは、信号ｈ_POSをオンにして信号ｈ_BKDをオフにする。これにより、遮断部２４２によって高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への制御誤差入力が遮断される。一方、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を有効にするときは、信号ｈ_POSをオフにして信号ｈ_BKDをオンにする。これにより、遮断部２４１によって低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への制御誤差入力が遮断される。

高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えでは、まず、入力部２１１から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示すモード信号が入力される。この場合、切替制御部（Ｈ）２２０は、上述した差分θ_errが予め設定された閾値以下である場合に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を有効とし、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を無効とする。

一方、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えでは、切替制御部（Ｈ）２２０は、上述した差分θ_errの値に関わりなく、当該切り替えを行う。即ち、入力部２１１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を示すモード信号が入力された場合、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を有効とし、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を無効とする。

１．２．２）高バックドライバビリティ駆動制御部（Ｋ_BKD）
本節では、高バックドライバビリティ駆動制御部（Ｋ_BKD）の詳細について説明する。図５は、図１に示す高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の機能構成の一例を示す図である。この図５において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

図５に示す高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、インナーループで力制御を行い、アウターループで位置制御を行う二重ループ系の構成となっている。この図５に示す構成により、連続体ロボット（Ｐ_n）１００に対してバックドライバビリティが高く、同時に位置決めを可能とする制御系が構成される。

図５に示す高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、位置制御部（Ｋ_SV）２５２１、遮断部２５２２、及び、力制御部（Ｋ_F）２５２３を有して構成されている。また、図５に示すＰ_nは、（２４）式及び（２７）式に示した連続体ロボット１００の動的モデルＰ_n（拡大系）を表している。

連続体ロボット１００の動的モデルＰ_n（拡大系）と力制御部（Ｋ_F）２５２３からなるインナーループでは、発生力の目標値ｒｅｆ_Fと発生力Ｆとの差分を取ることで誤差を演算する。力制御部（Ｋ_F）２５２３は、その誤差を補償する制御入力としてモータトルクＴ_mを出力する。即ち、図５に示す高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、ワイヤに印加される力の目標値とワイヤに印加された力の検出値との誤差を補償する機能を備える。このフィードバックループは、回転直動変換機構の等価慣性を補償することと等価であり、これにより連続体ロボット（Ｐ_n）１００のバックドライバビリティを向上することを可能とする。また、本実施形態では、このフィードバックによる閉ループ系をＧ_clと表す。このＧ_clと位置制御部（Ｋ_SV）２５２１からなるアウターループでは、位置の目標値ｒｅｆ_zと変位ｚ_t2との差分を取ることで誤差を演算する。位置制御部（Ｋ_SV）２５２１は、その誤差を補償する制御入力として発生力に対する目標値ｒｅｆ_Fを出力する。本実施形態では、力制御部（Ｋ_F）２５２３には、以下の（３２）式で示すＰＩＤ制御系を用いる。
（３２）式において、Ｆ_zｉ1、Ｆ_zd1は、それぞれ、積分制御器、微分制御器の零交
差周波数である。また、前章で示した高次モードに対する安定化のために、折点周波数２００Ｈｚの２次のローパスフィルタをＰＩＤ制御系に結合する。回転直動変換機構を用いることによる連続体部分である湾曲可能部１１０の極、零点の変動と、その零点付近で減衰係数ｃ_w2により変動する位相特性に対して、安定な力制御部（Ｋ_F）２５２３を設計するためには、次のように設計する。具体的に、開ループ伝達関数Ｐ_nＫ_Fを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する。

図６は、図５に示す力制御部（Ｋ_F）２５２３のボード線図と、図５に示す開ループ伝達関数Ｐ_nＫ_Fのボード線図である。具体的に、図６は、Ｋ_p、Ｆ_zｉ、Ｆ_zdを、それぞ
れ、２．２×１０Ｅ－３、１．０、９５とし、図６（ａ）に制御帯域を約３０Ｈｚとする力制御部（Ｋ_F）２５２３のボード線図を示し、図６（ｂ）に開ループ伝達関数Ｐ_nＫ_Fのボード線図を示す。

また、位置制御部（Ｋ_SV）２５２１には、以下の（３３）式で示すＰＩＤ制御系を用いる。
（３３）式において、Ｆ_zｉ2、Ｆ_zd2は、それぞれ、積分制御器、微分制御器の零交
差周波数である。本実施形態においては、連続体ロボット１００の動的モデルＰ_n（拡大系）と力制御部（Ｋ_F）２５２３との閉ループ伝達関数Ｇ_clを用いて、開ループ伝達関数Ｇ_clＫ_SVを導出し、その応答からゲイン余裕、位相余裕が十分であるように設計する。また、折点角周波数５０Ｈｚの１次のローパスフィルタをＰＩＤ制御系に結合する。

図７は、図５に示す位置制御部（Ｋ_SV）２５２１のボード線図と、図５に示す開ループ伝達関数Ｇ_clＫ_SVのボード線図である。具体的に、図７は、Ｋ_ps、Ｆ_zｉs、Ｆ_zdsを、
それぞれ、５×１０Ｅ３、０．０５、１００とし、図７（ａ）に制御帯域を約３．５Ｈｚとする位置制御部（Ｋ_SV）２５２１のボード線図と、図７（ｂ）に開ループ伝達関数Ｇ_clＫ_SVのボード線図を示す。

１．２．３）低バックドライバビリティ駆動制御部（Ｋ_POS）
高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、二重ループ制御系により、位置決め性能の低下を抑えたうえで、バックドライバビリティを向上させることができる。しかしながら、二重ループ制御系は、インナーループに力制御部（Ｋ_F）２５２３のノイズや高次振動を含む。そこで、バックドライバビリティの必要性が低い状況では、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１としてワイヤ把持機構の変位ｚ_t2を制御量とする位置制御系を用いる。具体的には、以下の（３４）式で示すＰＩＤ制御系を用いる。
（３４）式において、Ｆ_zｉ3、Ｆ_zd3は、それぞれ、積分制御器、微分制御器の零
交差周波数である。

図８は、図１に示す低バックドライバビリティ駆動制御部（Ｋ_POS）２５１のボード線図と、閉ループ系Ｇ_clとの開ループ伝達関数Ｇ_clＫ_POSのボード線図である。具体的に、図８は、Ｋ_p3、Ｆ_zｉ3、Ｆ_zd3を、それぞれ、２．３５×１０Ｅ－４、０．５、１００
とし、図８（ａ）に制御帯域を約３．５Ｈｚとする低バックドライバビリティ駆動制御部（Ｋ_POS）２５１のボード線図と、図８（ｂ）に開ループ伝達関数Ｇ_clＫ_POSのボード線図を示す。

１．３）シミュレーション
前章で説明した駆動制御部と、（２４）式及び（２７）式に示した連続体ロボット１００の動的モデルＰ_n（拡大系）を用いてシミュレーションを行う。

図９は、本発明の第１の実施形態におけるシミュレーションの模式図である。この図９において、図２に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

操作者は、台形状の目標軌道を与え、１．５秒で－４５度まで湾曲可能部１１０を湾曲させ、３秒まで停止し、その後０度まで復帰させる。このとき、図９に示すように湾曲可能部１１０の湾曲角度が－３０度となる角度で、湾曲可能部１１０の先端と弾性体である障害物９００とが接触するシミュレーションを行う。この際、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を用いて操作を開始し、また、発生力Ｆは監視可能であるものとする。そのため、操作者は、発生力Ｆが±２Ｎ（所定範囲）を超えると駆動するワイヤに負荷がかかっていると判断し、入力部２１１からのモード信号により高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替えるものとする。また、操作者は、０度への復帰操作では、発生力Ｆが±２Ｎ以内（所定範囲内）になると、入力部２１１からのモード信号により低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替えるものとする。なお、本実施形態においては、発生力Ｆが±２Ｎ（所定範囲）を超えるか否かの判断を操作者が行う例を示しているが、本発明においてはこの形態に限定されるものではない。例えば、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００において発生力Ｆを検出し、連続体ロボットの制御システム１０－１が、発生力Ｆの検出結果に基づいて入力部２１１からモード信号を発生させる形態も、本発明に適用可能である。

図１０は、本発明の第１の実施形態におけるシミュレーションの応答特性を示す図である。具体的に、図１０（ａ）はワイヤ把持機構の変位ｚ_t2を示し、図１０（ｂ）は湾曲可能部１１０の湾曲角度θを示し、図１０（ｃ）は湾曲可能部１１０の先端に加わる外乱トルクを示し、図１０（ｄ）は発生力Ｆを示し、図１０（ｅ）は制御入力Ｔ_mを示す。

図１０において、前章で示した本実施形態の制御系による応答を実線で示し、比較例としてバックドライバビリティの切り替えに条件を課さない制御系（以下、無条件切り替え制御系と略す）の応答を破線で示す。また、図１０（ａ）にアウターループに与えるワイヤ把持機構の目標変位ｒｅｆ_zを点線で示し、図１０（ｄ）に位置制御部（Ｋ_SV）２５２１が出力する制御入力である発生力目標値ｒｅｆ_Fを点線で示す。

図１０（ｂ）に示すように、シミュレーション開始後１秒で湾曲可能部１１０の湾曲角度θが－３０度に達し、湾曲可能部１１０の先端が障害物９００に接触するため、図１０（ｃ）に示すように外乱トルクを受け始める。このとき、図２を用いて説明したワイヤ張力検出機構のばねが伸びることにより、外乱を検知する。本実施形態では、ばねｋ_t2に発生する力がワイヤ把持機構に及ぼす力を発生力Ｆと定義しているため、図１０（ｄ）に示すように負の力として現れる。図１０（ｄ）より、約１．２秒で発生力は、－２Ｎに達し、操作者によって入力部２１１を介して高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えが行われる。

また、図１０（ｄ）に点線で示すように、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の力制御部（Ｋ_F）２５２３に対する初期目標値は、０である。この目標値に追従するため、図１０（ｅ）に示すように、力制御部（Ｋ_F）２５２３は、正の制御入力をモータに与え、モータが正方向に回転させる。これにより、図１０（ａ）に示すように、ワイヤ把持機構はｚ軸正方向に変位する。その結果、図１０（ｂ）に示すように、連続体部分である湾曲可能部１１０の先端は外乱トルクと同方向に湾曲し、外乱トルクに応じて連続体ロボット（Ｐ_n）１００がバックドライブする制御が実現している。

次に、インナーループである力制御系よりも帯域の低いアウターループである位置制御系の応答が立ち上がる。図１０（ｄ）の点線に示すように、位置制御系の制御入力である発生力の目標値が大きくなることにより、操作者の指令値への追従が行われる。しかしながら、インナーループにより回転直動変換機構の等価慣性を補償し低減しているため、バックドライバビリティが高く、図１０（ａ）に示すように緩やかな追従となる。これにより、発生力、即ちワイヤへの負荷を低く抑えることができ、連続体ロボットの損傷を防ぐことができる。そして、３秒より０度への復帰動作を開始すると、図１０（ｄ）に示すように、約３．１秒で発生力Ｆが２Ｎを下回る。

そこで、操作者は、入力部２１１を介して低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替え指令を発する。しかしながら、図１０（ｂ）より、湾曲可能部１１０の湾曲角度と目標角度との差が大きいため、切替制御部（Ｈ）２２０は、切り替えを行わない。そして約３．３秒でその差が０．５度（閾値）以内となるため、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１へと切り替える。そのため、図１０（ｂ）より、湾曲可能部１１０の湾曲角度θは、滑らかに指令値に収束しつつ０度に復帰していることがわかる。また、図１０（ｄ）より、切り替え時に発生力Ｆに大きな変動が生じていないことがわかる。

一方、図１０（ｂ）に破線で示す無条件切り替え制御系では、約３．１秒において低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えを許容し、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１は、急峻に目標角度へと追従する。このときの目標角度は、未だ障害物９００への接触が発生してしまう角度である。そのため、図１０（ｃ）に示すように、障害物９００より大きな外乱トルクを受けてしまう。これは、図１０（ｄ）に示すようにワイヤに及ぼす力が大きく、連続体ロボットが損傷する可能性が高い。なお、本実施形態では、切り替えに用いる湾曲可能部１１０の湾曲角度は、直接の観測量ではなく、（３０）式に示した逆運動学演算により求めている。

図１１は、本発明の第１の実施形態におけるシミュレーションの応答特性を示す図である。具体的に、図１１では、湾曲可能部１１０の湾曲角度を実線で示し、逆運動学演算による湾曲角度を破線で示している。本実施形態の逆運動学演算は、ワイヤの伸縮などは考慮されておらず誤差を含むが、切り替え時の安定性確保には影響がないことがわかる。

第１の実施形態に係る連続体ロボットの制御システム１０－１は、ワイヤ１１１が駆動されることによって湾曲可能に構成された湾曲可能部１１０と、ワイヤ１１１を駆動する駆動部であるアクチュエータ１８０を備える連続体ロボット１００－１を含む。そして、連続体ロボットの制御システム１０－１は、基準軸１０１に対する湾曲可能部１１０の湾曲角度からワイヤ１１１の駆動量を算出するための運動学演算を行う運動学演算部（Kinematics）２３０を含む。また、連続体ロボットの制御システム１０－１は、上述した運動学演算の結果に基づいて駆動部によるワイヤ１１１の駆動量を制御する第１の駆動制御部である低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を含む。また、連続体ロボットの制御システム１０－１は、湾曲可能部１１０が外力を受けたときに低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御の場合よりも駆動部が逆方向に駆動されやすい第２の駆動制御部である高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を含む。この高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２は、上述した運動学演算の結果に基づいて駆動部によるワイヤ１１１の駆動量を制御する。そして、連続体ロボットの制御システム１０－１は、入力部２１１から入力される選択する駆動制御部を示すモード信号に応じて、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２とを切り替える制御を行う切替制御部（Ｈ）２２０を含む。
かかる連続体ロボットの制御システム１０－１の構成によれば、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行うことができる。

さらに、連続体ロボットの制御システム１０－１は、上述した運動学演算とは逆の演算であってワイヤ１１１の駆動量から湾曲可能部１１０の湾曲角度を算出するための逆運動学演算を行う逆運動学演算部（ＩＫ）２７０を含む。そして、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、入力部２１１から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示すモード信号が入力された場合には、運動学演算部（Kinematics）２３０に入力された湾曲可能部の湾曲角度と逆運動学演算部（ＩＫ）２７０から出力された湾曲可能部の湾曲角度との差分θ_errが閾値以下である場合に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替える制御を行う。
かかる連続体ロボットの制御システム１０－１の構成によれば、当該切り替え時の目標値への急峻な追従が抑えられ、連続体ロボット自体や脆弱な対象物の損傷を防ぐことが実現できる。

（第２の実施形態）
次に、本発明の第２の実施形態について説明する。なお、以下に記載する第２の実施形態の説明では、上述した第１の実施形態と共通する事項については説明を省略し、上述した第１の実施形態と異なる事項について説明を行う。

上述した第１の実施形態では、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えは、湾曲可能部の湾曲角度における差分θ_errが閾値以下の十分小さいことを条件とした。これにより、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替え時に目標値への急峻な追従が抑えられ、連続体ロボット自体や周辺に損傷を与えることを防ぐことができることを説明した。しかしながら、第１の実施形態において、シミュレーションの応答特性を示す図１０（ｄ）及び図１０（ｅ）により、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替え時に、力制御部（Ｋ_F）２５２３に対する初期目標値は０であるため、モータへの制御入力はピークが生じていた。そこで、第２の実施形態では、切り替え時の制御入力のピークを抑える制御系を示す。

２．１）制御系設計
図１２は、本発明の第２の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図１２（ａ）に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－２」として記載する。この図１２（ａ）において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

図１２（ａ）に示す連続体ロボットの制御システム１０－２は、図１に示す連続体ロボットの制御システム１０－１の遮断部２４１，２４２及び選択部２６０に替えて、凸補間部（ＣＩ）２８１～２８３を用いている。図１２（ｂ）は、図１２（ａ）に示す凸補間部（ＣＩ）２８１～２８３の内部構成の一例を示している。

図１２（ｂ）に示すように、凸補間部（ＣＩ）２８１～２８３は、２つの入力ｕ_in1及びｕ_in2をパラメータαを用いて連続的に補間し、出力ｕ_OUTを演算するブロックである。この凸補間部（ＣＩ）２８１～２８３は、以下の（３５）式に示す演算を行う。

ｕ_OUT ＝ α・ｕ_in1＋（１－α）・ｕ_in2 (35)

本実施形態では、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替え時に、切替制御部（Ｈ）２２０は、パラメータαを０．２５秒かけて１から０へと変動させる。この切り替え時に、図１の選択部２６０に相当する図１２（ａ）の凸補間部（ＣＩ）２８３は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の出力と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の出力を連続的に補間する。即ち、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の制御入力が補間されることで連続的に遷移するため、急激にバックドライブする応答を抑えることができる。

一方、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替え時には、第１の実施形態と同様に、湾曲可能部の湾曲角度における差分θ_errが閾値以下であることを条件とする。このため、切替制御部（Ｈ）２２０は、パラメータαを即座に０から１へと変動させてよい。

２．２）シミュレーション
上述した図１０に示す第１の実施形態と同様に、シミュレーションを行う。
図１３は、本発明の第２の実施形態におけるシミュレーションの応答特性を示す図である。図１３（ａ）～図１３（ｅ）には、第２の実施形態の制御系（以下、「切り替え制御系ＩＩ」と略す）による応答を実線で示し、比較例として第１の実施形態の制御系（以下
、「切り替え制御系Ｉ」と略す）による応答を破線で示す。

図１３（ｄ）より、約１．２秒で発生力は－２Ｎに達し、操作者によって入力部２１１を介して高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えが行われる。なお、本実施形態においては、発生力Ｆが±２Ｎ（所定範囲）を超えるか否かの判断を操作者が行う例を示しているが、本発明においてはこの形態に限定されるものではない。例えば、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００において発生力Ｆを検出し、連続体ロボットの制御システム１０－２が、発生力Ｆの検出結果に基づいて入力部２１１からモード信号を発生させる形態も、本発明に適用可能である。

また、図１３（ｄ）に点線で示すように、二重ループ制御系の力制御部（Ｋ_F）２５２３に対する初期目標値は０である。低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えが連続的に行われるため、図１３（ｅ）に示すように、切り替え制御系Ｉで生じていたような制御入力
のピークは抑えられていることがわかる。また、図１３（ｂ）に示すように切り替え制御系ＩＩの外乱トルクに対するバックドライブの応答性は、切り替え制御系Ｉに比べて若干
低いものの、図１３（ｄ）に示すように、発生力、即ちワイヤへの負荷は低く抑えることができる。本実施形態で示した切り替え制御系ＩＩにより、低バックドライバビリティ駆
動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の双方向の切り替えにおいて、急峻な応答を抑えることができ、連続体ロボットの損傷を防ぐことが可能であることがわかった。

第２の実施形態においても、第１の実施形態と同様に、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行うことができる。

さらに、第２の実施形態に係る連続体ロボットの制御システム１０－２では、図１の選択部２６０に相当する構成として凸補間部（ＣＩ）２８３を設けている。この凸補間部（ＣＩ）２８３は、選択する駆動制御部の出力を低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の出力と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の出力とで切り替える場合に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１の出力と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２の出力を連続的に補間する機能を備えている。
かかる凸補間部（ＣＩ）２８３によれば、急峻な応答を抑えることができ、連続体ロボットの損傷を防ぐことが可能である。

（第３の実施形態）
次に、本発明の第３の実施形態について説明する。なお、以下に記載する第３の実施形態の説明では、上述した第１及び第２の実施形態と共通する事項については説明を省略し、上述した第１及び第２の実施形態と異なる事項について説明を行う。

上述した第１及び第２の実施形態では、急峻な応答を抑えつつ、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２とを切り替える形態を示した。第３の実施形態では、複数の湾曲可能部１１０を有する連続体ロボット１００に対して、制御系が切り替わることを考慮する先頭追従制御のアルゴリズムを示す。

まず、複数の湾曲可能部１１０を有する連続体ロボット１００の運動学モデルの導出を説明し、次に制御系の導出を詳細に述べる。

１）モデリング
図１４は、本発明の第３の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。この図１４において、図２に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。以下、この図１４に示す連続体ロボット１００を「連続体ロボット１００－３」として記載する。

本章では、図１４に示す複数の湾曲可能部１１０－１～１１０－ｅを有する連続体ロボット１００－３のｘ－ｚ平面における運動学を導出する。図１４に示す複数の湾曲可能部１１０－１～１１０－ｅを有する連続体ロボット１００－３の動的モデルにおける符号の定義を以下に記載する。
ｌ_n：第ｎの湾曲可能部１１０－ｎの長さ
ｒ_n：第ｎの湾曲可能部１１０－ｎのワイヤガイドを通るワイヤからワイヤガイドの中心までの距離
ｅ：湾曲可能部１１０の数（総数）
θ_n：第ｎの湾曲可能部１１０－ｎの湾曲角度
ρ_n：第ｎの湾曲可能部１１０－ｎの曲率半径
θ_refn：第ｎの湾曲可能部１１０－ｎの目標湾曲角度（湾曲角度の目標値）
ｌ_pn：第ｎの湾曲可能部１１０－ｎのワイヤの駆動変位（駆動量）
ｘ_tn，ｚ_tn：第ｎの湾曲可能部１１０－ｎの遠位端の座標
ｚ_b：ロボット基台１４０の変位
ｌ_nv：先頭追従制御に用いる第ｎの湾曲可能部１１０－ｎの仮想的な長さ。

次に、図１４に示す複数の湾曲可能部１１０－１～１１０－ｅを有する連続体ロボット１００－３の運動学を導出する。まず、第１の湾曲可能部１１０－１のワイヤの駆動量ｌ_p1と第１の湾曲可能部１１０－１の湾曲角度θ₁との関係は、以下の（３６）式となる。

ｌ_p1 ＝ ｒ₁θ₁ (36)

続いて、第ｎの湾曲可能部１１０－ｎのワイヤの駆動量ｌ_pnと第ｎの湾曲可能部１１０－ｎの湾曲角度θ_nとの関係を導出する。ただし、ｎは、２以上とする。

図１５は、図１４に示す第ｎの湾曲可能部１１０－ｎの部分を抽出した図である。この図１５に示すように、第ｎの湾曲可能部１１０－ｎの原点Ｏを（ｘ_tn-1，ｚ_tn-1）とし、
θ_n-1方向及びその直交方向からなる相対座標系ｘ_n－ｚ_nをとる。

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

これより、第ｎの湾曲可能部１１０－ｎの湾曲角度θ_nは、ワイヤの駆動変位ｌ_pnによってのみ決まり、途中の湾曲可能部１１０の湾曲角度には依存しないことがわかる。

次に、経路との接触シミュレーションを行うための有限要素モデルを導出する。
連続体ロボットをｍ個の要素に分割し、１次元のばね系と簡略化しモデル化する。全体の剛性行列をＫ_FE、節点力ベクトル及び節点変位ベクトルをｆ及びδとし、それぞれ、外乱トルク、角変位に相当すると考えると、全系の剛性方程式は、以下の（４０）式となる。

ｆ ＝ Ｋ_FEδ (40)

ここで、連続体ロボットの第ｉ番目の要素の曲げ剛性をｋ_ｉとし、要素の剛性行列Ｋ_ｉ
を以下の（４１）式とする。

本実施形態では、湾曲可能部１１０の最遠位端に相当する有限要素モデルの要素を、１）低バックドライバビリティ駆動制御部（Ｋ_POS）２５１が選択されているときは拘束節点と考え、２）高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２が選択されているときは拘束のない節点と考える。そして、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１が選択されているとき、目標湾曲角度を拘束変位δ_Bとして与える。剛性行列を並び替えると、以下の（４２）式となる。

そして、連続体ロボットが経路から受ける外力をペナルティ法により求め、節点力ベクトルｆ_Aと節点変位ベクトルδ_Aを繰り返し更新する収束計算を行うことで、接触時の形状をシミュレーションする。

２）制御系設計
２．１）先頭追従制御
図１６は、図１４に示す連続体ロボット１００－３の先頭追従制御を説明するための図である。この図１６では、連続体ロボット１００－３が＋ｚ方向に進んでいく様子を示している。また、図１６では、複数の湾曲可能部１１０として、第１の湾曲可能部１１０－１、・・・、第ｅ－２の湾曲可能部１１０－（ｅ－２）、第ｅ－１の湾曲可能部１１０－（ｅ－１）、第ｅの湾曲可能部１１０－ｅを図示している。

ここで、先頭追従制御とは、図１６に示すように、最遠位端の第ｅの湾曲可能部１１０－ｅ（遠位湾曲可能部）が通る経路と同じ経路を、ロボット基台１４０と第ｅの湾曲可能部１１０－ｅとの間にある追従湾曲可能部が通るように制御する方法である。これによって、連続体ロボット１００－３は、狭小な空間をすり抜けるように進むことができる。先頭追従制御は、予め経路が定められていることは必須ではなく、例えば、最遠位端の第ｅの湾曲可能部１１０－ｅの湾曲角度を追従湾曲可能部に湾曲可能部の長さをもって連続的に伝播させればよい。この方法を用いると、操作者はジョイステックなどで最遠位端の湾曲可能部の湾曲角度とロボット基台１４０の進行量に対してのみ指令を与えることで、連続体ロボット１００－３を実時間で先頭追従制御することができる。

図１７は、図１６に示す連続体ロボット１００－３の先頭追従制御の一例を示す図である。この図１７には、横軸をロボット基台１４０の変位ｚ_bとし、縦軸を湾曲角度θとした先頭追従制御のグラフが示されている。この図１７において、「Ｌｅａｄｅｒ」は、図１６に示す最遠位湾曲可能部１１０－ｅを表している。また、図１７において、「１ｓｔ ｆｏｌｌｏｗｅｒ」は図１６に示す第１の追従湾曲可能部１１０－（ｅ－１）を表し、「２ｎｄ ｆｏｌｌｏｗｅｒ」は図１６に示す第２の追従湾曲可能部１１０－（ｅ－２）を表している。

この図１７では、点線で操作者の最遠位湾曲可能部１１０－ｅに対する湾曲角度指令を示し、太破線で第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度指令を示す。ロボット基台１４０の変位ａにおいて、湾曲角度指令ａｂが操作者により与えられると、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度はロボット基台１４０の変位ｃにおいて湾曲角度がｃｄとなるように自動生成されればよい。ここで、ロボット基台１４０の変位ｃは、距離ａｃが第１の追従湾曲可能部１１０－（ｅ－１）の長さｌとなるように決定されるが、実際の長さよりも短く設定してもよい。そして、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度指令は、連続体ロボットの制御システム１０に含まれる制御演算装置の記憶部に収納され、ロボット基台１４０の変位に応じて引き出される。しかしながら、この指令値では、ロボット基台１４０の変位がａとｃの間にある場合、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度は変化せず、ロボット基台１４０の変位ｃにおいて後続の湾曲可能部の湾曲角度指令が立ち上がるため、連続体ロボットは急峻な挙動を示す。そこで、本実施形態では、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度指令を、点ａと点ｄを結ぶように補間する。補間された第１の追従湾曲可能部１１０－（ｅ－１）の目標湾曲角度を図１７において実線で示す。追従湾曲可能部の数が２以上の場合は、上記の説明における第１の追従湾曲可能部１１０－（ｅ－１）を最遠位湾曲可能部１１０－ｅに置き換え、この処理を連続的に行えば全ての湾曲可能部１１０の湾曲角度指令値を求めることができる。例として、図１７では、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度指令を細破線で示している。

２．２）制御系のブロック線図
本実施形態では、上述した先頭追従制御中に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２とを切り替える制御系を設計する。本実施形態でも、第１の実施形態と同様に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えは、入力部２１１からのモード信号の入力により任意に可能とする。本実施形態でも、第１の実施形態と同様に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えは、湾曲可能部の湾曲角度における差分θ_errが閾値以下であることを条件とする。

図１８は、本発明の第３の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図１８に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－３」として記載する。この図１８において、図１及び図１２に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

連続体ロボットの制御システム１０－３は、図１と同様の構成として、連続体ロボット（Ｐ_n）１００、入力部２１１，２１２、切替制御部（Ｈ）２２０、運動学演算部（Kinematics）２３０、及び、逆運動学演算部（ＩＫ）２７０を有している。ここで、図１８に示す連続体ロボット（Ｐ_n）１００は、図１４に示す複数の湾曲可能部１１０を有する連続体ロボット１００－３であるものとする。さらに、連続体ロボットの制御システム１０－３は、入力部２１３、最遠位駆動制御機構（Ｋ_SWL）３１０、追従駆動制御機構（Ｋ_SWF）３２０、及び、追従制御部（ＦＴＬ）３３０を有している。

入力部２１１は、第１の実施形態と同様に、選択する駆動制御部を示すモード信号を切替制御部（Ｈ）２２０に対して入力する。入力部２１２は、基準軸１０１に対する最遠位湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θLを運動学演算部（Kinematics）２３０や追従制御部（ＦＴＬ）３３０等に対して入力する。入力部２１３は、ロボット基台１４０の目標変位ｚ_brefを追従制御部（ＦＴＬ）３３０に対して入力する。

最遠位駆動制御機構（Ｋ_SWL）３１０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この最遠位駆動制御機構（Ｋ_SWL）３１０は、最遠位湾曲可能部１１０－ｅの駆動を制御する構成部である。

追従駆動制御機構（Ｋ_SWF）３２０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この追従駆動制御機構（Ｋ_SWF）３２０は、追従湾曲可能部１１０（例えば、追従湾曲可能部１１０－（ｅ－１））の駆動を制御する構成部である。

追従制御部（ＦＴＬ）３３０は、前節に説明した先頭追従制御を行う構成部である。この追従制御部（ＦＴＬ）３３０は、最遠位湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θLとロボット基台１４０の目標変位ｚ_brefから、追従湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θFを生成する。さらに、追従制御部（ＦＴＬ）３３０は、切替制御部（Ｈ）２２０からの信号ｈ_POS及びｈ_BKD、並びに、後述する最遠位湾曲可能部及び追従湾曲可能部の湾曲角度θ_IKを入力とするが、詳細な動作は次節で説明する。また、運動学演算部（Kinematics）２３０は、最遠位湾曲可能部及び追従湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θL及びｒｅｆ_θFから、最遠位湾曲可能部及び追従湾曲可能部のワイヤ把持機構の目標変位ｒｅｆ_zL及びｒｅｆ_zFを、以下の（４３）式により出力する。

逆運動学演算部（ＩＫ）２７０は、最遠位湾曲可能部及び追従湾曲可能部のワイヤ把持機構の変位ｚ_t2L及びｚ_t2Fから、最遠位湾曲可能部及び追従湾曲可能部の湾曲角度θ_IKL及びθ_IKFを、それぞれ、以下の（４４）式及び（４５）式により出力する。

本実施形態では、以下の（４６）式に示す、最遠位湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θLと最遠位湾曲可能部の湾曲角度θ_IKLとの差分θ_errが、切替制御部（Ｈ）２２０に入力される。

θ_err ＝ ｒｅｆ_θL－θ_IKL (46)

第１の実施形態と同様に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えでは、切替制御部（Ｈ）２２０は、最遠位湾曲可能部の湾曲角度における差分θ_errが閾値以下である場合に、信号ｈ_OUT、信号ｈ_POS及び信号ｈ_BKDによって制御系を切り替える。また、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えでは、切替制御部（Ｈ）２２０は、最遠位湾曲可能部の湾曲角度における差分θ_errの値に関わりなく、制御系を切り替える。

なお、図１８では、追従湾曲可能部を添え字「Ｆ」に代表させて図示している。追従湾曲可能部が複数ある場合には、同じブロック構造が並列に並べばよい。

２．３）バックドライバビリティの切り替えを伴う先頭追従制御
図１９は、本発明の第３の実施形態を示し、図１６に示す連続体ロボット１００－３の先頭追従制御の一例を示す図である。具体的に、図１９（ａ）～図１９（ｃ）は、先頭追従制御中のバックドライバビリティの切り替えに対応する追従制御部（ＦＴＬ）３３０について、３通りのアルゴリズムをグラフに示した図である。この図１９（ａ）～図１９（ｃ）においても、上述した図１７と同様に、横軸をロボット基台１４０の変位ｚ_bとし、縦軸を湾曲角度θとした先頭追従制御のグラフが示されている。また、図１９（ａ）～図１９（ｃ）においても、上述した図１７と同様に、「Ｌｅａｄｅｒ」は、図１６に示す最遠位湾曲可能部１１０－ｅを表している。また、図１９（ａ）～図１９（ｃ）において、「１ｓｔ ｆｏｌｌｏｗｅｒ」は図１６に示す第１の追従湾曲可能部１１０－（ｅ－１）を表し、「２ｎｄ ｆｏｌｌｏｗｅｒ」は図１６に示す第２の追従湾曲可能部１１０－（ｅ－２）を表している。即ち、図１９（ａ）～図１９（ｃ）は、点線で操作者の最遠位湾曲可能部１１０－ｅに対する湾曲角度指令を示し、第１の追従湾曲可能部１１０－（ｅ－１）及び第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度指令をそれぞれ実線及び破線で示す。また、図１９（ａ）～図１９（ｃ）において、ｌ₂は第１の追従湾曲可能部１１０－（ｅ－１）の長さを示し、ｌ₁は第２の追従湾曲可能部１１０－（ｅ－２）の長さを示している。

図１９（ａ）は、ロボット基台１４０の変位Ａにおいて、制御系が高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替えられると、最遠位湾曲可能部、第１及び第２の追従湾曲可能部の湾曲角度は、それぞれ、ＡＢ、ＡＢ'及びＡＢ''へとバックドライブしている状態を図示している。この場合、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度は、ロボット基台１４０の変位Ｃにおいて、湾曲角度がＣＤとなるように自動生成されればよい。ここで、ロボット基台１４０の変位Ｃは、距離ＡＣが第１の追従湾曲可能部１１０－（ｅ－１）の長さｌ₂となるように決定される。ここで、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度は、３通りのアルゴリズムが考えられる。

まず、図１９（ａ）に示す第１のアルゴリズムでは、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度は、ロボット基台１４０の変位Ｃにおいて湾曲角度がＣＤ''となるように自動生成されればよい。ここで湾曲角度ＣＤ''は、湾曲角度ＡＢ'と同一であり、１つ前方の湾曲可能部の湾曲角度に追従することを表す。そして、ロボット基台１４０の変位Ｆにおいて、湾曲角度がＦＥとなるように自動生成されればよい。ここで、ロボット基台１４０の変位Ｆは、距離ＣＦが第２の追従湾曲可能部１１０－（ｅ－２）の長さｌ₁となるように決定される。また、湾曲角度ＦＥは、湾曲角度ＡＢと同一であり、１つ前方の湾曲可能部の湾曲角度を経て最遠位湾曲可能部の湾曲角度に追従することを表す。

続いて、図１９（ｂ）に示す第２のアルゴリズムでは、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度は、ロボット基台１４０の変位Ｆにおいて湾曲角度がＦＥとなるように自動生成されればよい。湾曲角度ＦＥは、湾曲角度ＡＢと同一であり、最遠位湾曲可能部の湾曲角度に直接的に追従することを表す。

続いて、図１９（ｃ）に示す第３のアルゴリズムでは、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度は、ロボット基台１４０の変位Ｃにおいて湾曲角度がＣＤ''となるように自動生成されればよい。ここで、湾曲角度ＣＤ''は、湾曲角度ＡＢ''と同一であり、ロボット基台１４０が１つ前方の湾曲可能部の長さｌ₂を前進する間は、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度を変化させないことを表す。そして、ロボット基台１４０の変位Ｆにおいて湾曲角度ＦＥとなるように自動生成されればよい。

なお、本節では、実際の湾曲可能部の長さｌ_nを用いて説明したが、実際の湾曲可能部の長さよりも短い仮想的な長さｌ_nvを用いてもよい。

３）シミュレーション
前章で設計した制御系と、（４２）式で導出した有限要素モデルを用いて、シミュレーションを行う。第２の追従湾曲可能部１１０－（ｅ－２）は、図１９（ａ）に示す先頭追従制御の第１のアルゴリズムを用いて湾曲角度を求める。ここでは、連続体ロボット１００－３の湾曲可能部１１０の数は３とし、最遠位湾曲可能部の長さｌ₃、第１及び第２の追従湾曲可能部の長さｌ₂及びｌ₁を、それぞれ、以下の（４７）式とする。

ｌ₃＝０．０２１、ｌ₂＝０．０２、ｌ₁＝０．１５９ (47)

さらに、第２の追従湾曲可能部の遠位端から０．０２ｍは、最遠位湾曲可能部及び第１の追従湾曲可能部と同様の低剛性としている。そこで、図１９（ａ）に示す第１のアルゴリズムにおいて第２の追従湾曲可能部に用いる長さは、以下の（４８）式で示す実際より短い長さｌ_1vとしている。

ｌ_1v＝０．０２ (48)

図２０は、本発明の第３の実施形態におけるシミュレーションの応答特性を示す図である。具体的に、図２０（ａ）～図２０（ｆ）にロボット基台１４０の前進に伴う連続体ロボット１００－３の形状を太実線で示し、経路の側壁を細実線で示す。

図２０（ａ）において、操作者は、最遠位湾曲可能部の湾曲角度に指令を与え、さらに第１の追従湾曲可能部の湾曲角度が追従制御部（ＦＴＬ）３３０により生成されて制御されている。

図２０（ｂ）において、操作者は、最遠位湾曲可能部の湾曲角度を０度に戻すことで経路に沿わせている。第１の追従湾曲可能部に続いて第２の追従湾曲可能部の湾曲角度が追従制御部（ＦＴＬ）３３０により生成され、それぞれ、１つ遠位の湾曲可能部の湾曲角度に追従している。

図２０（ｃ）において、操作者は、最遠位湾曲可能部の湾曲角度を進行方向左方に向ける動作指令を開始している。

図２０（ｄ）において、操作者は、最遠位湾曲可能部の湾曲角度を操作し経路に沿わせ、第１及び第２の追従湾曲可能部は、それぞれ、遠位の湾曲可能部の湾曲角度に追従している。しかしながら、第１の追従湾曲可能部は、湾曲角度が０度であるため、右側の側壁に押し付けられ、湾曲可能部の基端側は大曲率の変形をしていることがわかる。これは連続体ロボット１００－３の構造に大きな負荷を与えるため、操作者は、制御系を高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２へと切り替える。

図２０（ｅ）は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えによる形状を示している。第１の追従湾曲可能部の曲率が区間全域にわたって一定に近づいていることがわかる。これにより、連続体ロボット１００－３の構造に対する負荷が軽減される。この後、操作者は、再び低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替えると、バックドライブによる形状を初期姿勢とし先頭追従制御を再開することができる。

図２０（ｆ）は、先頭追従制御を再開している形状を示す。第１及び第２の追従湾曲可能部は、それぞれ、遠位の湾曲可能部に追従している。

図２１は、本発明の第３の実施形態におけるシミュレーションの応答特性を示す図である。具体的に、図２１（ａ）は、横軸をロボット基台１４０の位置とし、先頭追従制御における各湾曲可能部の湾曲角度を示している。また、図２１（ｂ）は、横軸をロボット基台１４０の位置とし、節点力ベクトルの二乗平均平方根の応答を示している。ここで、節点力ベクトルの二乗平均平方根は、側壁から与えられる外乱トルクに相当する。

図２１（ａ）では、最遠位湾曲可能部、第１及び第２の追従湾曲可能部の湾曲角度を、それぞれ、実線、破線、一点鎖線で示している。制御開始時は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を用いる先頭追従制御が行われる。そして、ロボット基台１４０の変位ｚ_bが０．０８ｍにおいて、制御系が高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替えられると、それぞれの湾曲可能部がバックドライブし、側壁からの外力を受けづらい湾曲形状へと変動する。特に、第１の追従湾曲可能部の湾曲角度が０度から－２２度へと大きく変動していることがわかる。そして、再び、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１へと切り替えられ、最遠位湾曲可能部の湾曲角度は、操作者により－７０度へと操作される。第１及び第２の追従湾曲可能部は、それぞれ、長さｌ₂及びｌ_1vをもって、それぞれの遠位の湾曲可能部の湾曲角度へと追従していることがわかる。

図２１（ｂ）には、本実施形態で示した制御系の切り替えを伴う先頭追従制御による節点力ベクトルの応答を実線で示し、比較例として低バックドライバビリティ駆動制御部（Ｋ_POS）２５１のみを用いる先頭追従制御による応答を破線で示す。本実施形態の場合には、ロボット基台１４０の変位ｚ_bが０．０８ｍにおいて制御系が高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２へと切り替えられ、連続体ロボット１００－３の形状が経路に倣うことにより、外乱トルクが大幅に低減することがわかる。再び、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１へと切り替えられ、ロボット基台１４０の進行が再開されると、ロボット基台１４０の変位ｚ_bが０．１２ｍにおいて、若干比較例として挙げている低バックドライバビリティ駆動制御部（Ｋ_POS）２５１のみを用いる先頭追従制御による外乱トルクを上回るものの、全体として外乱トルクを抑えつつ先頭追従制御を継続していることがわかる。

第３の実施形態においても、第１の実施形態と同様に、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行うことができる。

さらに、第３の実施形態に係る連続体ロボットの制御システム１０－３では、最遠位湾曲可能部及び追従湾曲可能部ごとに、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１及び高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を含む駆動制御機構（最遠位駆動制御機構（Ｋ_SWL）３１０及び追従駆動制御機構（Ｋ_SWF）３２０）を設けている。そして、追従制御部（ＦＴＬ）３３０は、運動学演算部（Kinematics）２３０に入力される遠位湾曲可能部の湾曲角度ｒｅｆ_θLと入力部２１３から入力されるロボット基台１４０の変位ｚ_bとに基づいて、追従湾曲可能部の湾曲角度ｒｅｆ_θFを運動学演算部（Kinematics）２３０に出力している。
かかる構成によれば、バックドライバビリティの切り替えを伴う先頭追従制御を実現することができる。

（第４の実施形態）
次に、本発明の第４の実施形態について説明する。なお、以下に記載する第４の実施形態の説明では、上述した第１～第３の実施形態と共通する事項については説明を省略し、上述した第１～第３の実施形態と異なる事項について説明を行う。

上述した第３の実施形態では、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１と高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を先頭追従制御中に切り替える制御系を設計し、追従制御部（ＦＴＬ）３３０での通りのアルゴリズムを示した。この際、第３の実施形態では、ロボット基台１４０が前進する制御を扱った。先頭追従制御の前進時では、最遠位湾曲可能部は操作者により目標湾曲角度が与えられ、追従湾曲可能部は各湾曲可能部において遠位の湾曲可能部の湾曲角度を伝播されればよい。ここで、前進時のロボット基台１４０の変位における、最遠位湾曲可能部を含む全湾曲可能部の目標湾曲角度を追従制御部（ＦＴＬ）３３０の記憶部に記憶させておくことができる。これを用いると、ロボット基台１４０が後進するときは、ロボット基台１４０の変位に対する各湾曲可能部の目標湾曲角度を記憶部から読み出すことで、操作者が湾曲指令を与えることなく連続体ロボットは経路に沿って後進することができる。そこで、本実施形態では、ロボット基台１４０の前進時に制御系が切り替わる際に、ロボット基台１４０の後進を考慮する追従制御部（ＦＴＬ）３３０のアルゴリズムを示す。

図２２は、本発明の第４の実施形態を示し、図１６に示す連続体ロボット１００－３の先頭追従制御の一例を示す図である。具体的に、図２２（ａ）～図２２（ｃ）は、先頭追従制御中の制御モードの変更に対する追従制御部（ＦＴＬ）３３０について、３通りのアルゴリズムをグラフに示した図である。この図２２（ａ）～図２２（ｃ）では、前進のためのアルゴリズムは、図１９（ａ）に示す第３の実施形態における第１のアルゴリズムを図示しているが、図１９（ｂ）に示す第２のアルゴリズムや図１９（ｃ）に示す第３のアルゴリズムを用いてもよい。

この図２２（ａ）～図２２（ｃ）においても、上述した図１９と同様に、横軸をロボット基台１４０の変位ｚ_bとし、縦軸を湾曲角度θとした先頭追従制御のグラフが示されている。また、図２２（ａ）～図２２（ｃ）においても、上述した図１９と同様に、「Ｌｅａｄｅｒ」は、図１６に示す最遠位湾曲可能部１１０－ｅを表している。また、図２２（ａ）～図２２（ｃ）において、「１ｓｔ ｆｏｌｌｏｗｅｒ」は図１６に示す第１の追従湾曲可能部１１０－（ｅ－１）を表し、「２ｎｄ ｆｏｌｌｏｗｅｒ」は図１６に示す第２の追従湾曲可能部１１０－（ｅ－２）を表している。即ち、図２２（ａ）～図２２（ｃ）は、点線で操作者の最遠位湾曲可能部１１０－ｅに対する湾曲角度指令を示し、第１の追従湾曲可能部１１０－（ｅ－１）及び第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度指令をそれぞれ実線及び破線で示す。また、図２２（ａ）～図２２（ｃ）において、ｌ₃は最遠位湾曲可能部１１０－ｅの長さを示し、ｌ₂は第１の追従湾曲可能部１１０－（ｅ－１）の長さを示し、ｌ₁は第２の追従湾曲可能部１１０－（ｅ－２）の長さを示している。

図１９に示す第３の実施形態と同様に、図２２においても、ロボット基台１４０の変位Ａにおいて、制御系が高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替えられると、最遠位湾曲可能部、第１及び第２の追従湾曲可能部の湾曲角度は、それぞれ、ＡＢ、ＡＢ'及びＡＢ''へとバックドライブしている状態を図示している。

ロボット基台１４０の後進では、第２の追従湾曲可能部１１０－（ｅ－２）の湾曲角度は、ロボット基台１４０の変位Ｑにおいて、湾曲角度がＱＴ''となるように自動生成されればよい。ここで、ロボット基台１４０の変位Ｑは、距離ＡＱが最遠位湾曲可能部１１０－ｅの長さｌ₃となるように決定される。ここで、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度は、３通りのアルゴリズムが考えられる。

まず、図２２（ａ）に示す第１のアルゴリズムでは、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度は、ロボット基台１４０の変位ＱにおいてＱＴ'となるように自動生成されればよい。ここで、湾曲角度ＱＴ'は、湾曲角度ＡＢ'と同一である。そして、ロボット基台１４０の変位Ｒにおいて、第１の追従湾曲可能部１１０－（ｅ－１）の湾曲角度がＲＵ'となるように自動生成されればよい。ここで、ロボット基台１４０の変位Ｒは、距離ＲＱが第１の追従湾曲可能部１１０－（ｅ－１）の長さｌ₂となるように決定される。また、湾曲角度ＲＵ'は、湾曲角度ａｂと同一である。次に、最遠位湾曲可能部１１０－ｅの湾曲角度は、ロボット基台１４０の変位Ｒにおいて湾曲角度がＲＵとなるように自動生成されればよい。ここで、湾曲角度ＲＵは、湾曲角度ＡＢと同一である。そして、ロボット基台１４０の変位Ｓにおいて、湾曲角度がＳＶとなるように自動生成されればよい。ここで、ロボット基台１４０の変位Ｓは、距離ＳＲが第２の追従湾曲可能部１１０－（ｅ－２）の長さｌ₁となるように決定される。また、湾曲角度ＳＶは、湾曲角度ａｂと同一である。このように生成することで、あたかもロボット基台１４０の変位Ｓから先頭追従制御により、最遠位湾曲可能部、第１及び第２の追従湾曲可能部の順に湾曲しつつ前進したような目標湾曲角度を生成できる。

続いて、図２２（ｂ）に示す第２のアルゴリズムでは、図２２（ａ）に示す第１のアルゴリズムから、それぞれ、最遠位湾曲可能部及び第１の追従湾曲可能部の通過点となる点Ｕ及び点Ｔ'を省略する。これにより、後退時に、最遠位湾曲可能部及び第１の追従湾曲可能部は、それぞれ、点Ｖ及び点Ｕ'に直接的に変化する目標湾曲角度が生成される。

続いて、図２２（ｃ）に示す第３のアルゴリズムでは、全ての湾曲可能部が、ロボット基台１４０の変位Ｑにおいて湾曲角度ＱＴ''に直接的に変化する目標湾曲角度が生成される。

第４の実施形態においても、上述した第３の実施形態と同様の効果を奏する。

（第５の実施形態）
次に、本発明の第５の実施形態について説明する。なお、以下に記載する第５の実施形態の説明では、上述した第１～第４の実施形態と共通する事項については説明を省略し、上述した第１～第４の実施形態と異なる事項について説明を行う。

上述した第１～第４の実施形態では、ｘ－ｚ平面内駆動の連続体ロボット１００に対して制御系設計を行った。第５実施形態では、３次元立体駆動可能な連続体ロボット１００に対して制御系設計を行う。

図２３は、本発明の第５の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。以下、図２３に示す連続体ロボット１００を「連続体ロボット１００－５」として記載する。

図２３に示す連続体ロボット１００－５は、湾曲可能部１１００の遠位端１０６０における固定部１０２１～１０２３にワイヤ１０１１～１０１３が接続されている。また、ワイヤ１０１１～１０１３の近位端は、ワイヤ把持パイプ１０３１～１０３３が接続されている。ロボット基台１０４０には、第１の実施形態と同様に、ワイヤ１０１１～１０１３における各ワイヤごとに、ワイヤ把持機構（図２３では不図示）及びアクチュエータ（図２３では不図示）が設置されている。ここで、ワイヤ把持機構は、アクチュエータにワイヤ把持機構基台（図２３では不図示）を介して接続され、上下に移動可能である。ワイヤ把持パイプ１０３１～１０３３は、ワイヤ把持機構に接続され、アクチュエータで押引きすることにより姿勢が制御される。また、連続体ロボット１００－５は、ワイヤ１０１１～１０１３を案内するための部材であるワイヤガイド１０６１～１０６４を有する。このワイヤガイドは、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤガイド１０６１～１０６４は、ワイヤ１０１１に、固定部１０５０～１０５３において固定される。さらに、ワイヤ１０１１～１０１３の間隔と、ワイヤ把持パイプ１０３１～１０３３との間隔が異なってもよい。そのときには、径変換部１１９０をロボット基台１０４０に接続すればよい。また、図２３において、連続体ロボット１００－５の中心軸を破線で示す。

本実施形態では、ワイヤ１０１１～１０１３と、ワイヤガイド１０６１～１０６４からなる機構を連続体部分である湾曲可能部１１００とよぶ。また、不図示のアクチュエータには、回転モータと回転直動変換機構からなるアクチュエータユニットを用いるものとする。さらに、ワイヤ把持機構は、ワイヤの張力を検出する機能を持つものとする。そのためには、ワイヤ把持機構とアクチュエータとの間に上述したワイヤ把持機構基台を設け、ワイヤ把持機構基台をアクチュエータに接続し、ワイヤ把持機構基台とワイヤ把持機構の間をばねで接続する。このとき、ワイヤ把持機構がｚ軸方向にのみ変位するように、リニアガイドを設けることや、ばねに平行ばねを用いることが好ましい。そして、ばねの変位を計測することでワイヤの張力が検出可能となる。

以下に、本実施形態で使用する符号の定義を記載する。
ｌ_d：湾曲可能部１１００の中心軸の長さ
θ_n：基準軸１０１に対する湾曲可能部１１００の遠位端の湾曲角度
ζ_n：基準軸（ｘ軸）に対する湾曲可能部１１００の遠位端の旋回角度
θ_IK：逆運動学演算部（ＩＫ）２７０による湾曲可能部１１００の遠位端の湾曲角度
ζ_IK：逆運動学演算部（ＩＫ）２７０による湾曲可能部１１００の遠位端の旋回角度
ρ_n：湾曲可能部１１００の曲率半径

また、本実施形態では、ワイヤ１０１１～１０１３をｘｙ面内において反時計回りにａワイヤ、ｂワイヤ、ｃワイヤと呼び、そのａ～ｃワイヤ１０１１～１０１３の駆動変位（駆動量）をｌ_p1a，ｌ_p1b，ｌ_p1cとする。

図２４は、本発明の第５の実施形態に係る連続体ロボット１００のワイヤの配置の一例を示す図である。この図２４に示すように、ａ～ｃワイヤ１０１１～１０１３は、一片の長さをｒ_sとする正三角形の頂点に配置されており、位相角ξ_nは、ワイヤの配置を決定する角度である。本実施形態では、ξ₁＝０とする。

そして、本実施形態では、以下の仮定をおき、連続体ロボット１００－５の運動学を導出する。
［１］各湾曲可能部１１００において、ワイヤは曲率一定に変形する。
［２］ワイヤのねじり変形は考慮しない。
［３］ワイヤは長手方向に変形しない。
［４］ワイヤガイドとワイヤとの間の摩擦は考慮しない。

これにより、湾曲可能部１１００の遠位端の湾曲角度θ₁及び旋回角度ζ₁とするための、ａワイヤの駆動変位ｌ_p1a，ｂワイヤの駆動変位ｌ_p1b，ｃワイヤの駆動変位ｌ_p1cは、以下の（４９）式となる。

本実施形態では、第１の実施形態と同様にワイヤの伸縮を考慮しないため、ａ～ｃワイヤ１０１１～１０１３におけるワイヤ把持機構の目標変位ｒｅｆ_za、ｒｅｆ_zb及びｒｅｆ_zcは、以下の（５０）式となる。

ｒｅｆ_za＝ｌ_p1a、ｒｅｆ_zb＝ｌ_p1b、ｒｅｆ_zc＝ｌ_p1c (50)

また、逆運動学演算部（ＩＫ）２７０による逆運動学演算を、以下の（５１）式とおく。

（５１）式において、ワイヤ把持機構の変位についてｚ_t2c≠ｚ_t2bの場合には、以下の（５２）式となる。

（５１）式において、ワイヤ把持機構の変位についてｚ_t2c＝ｚ_t2bの場合には、以下の（５３）式となる。

（５１）式において、ワイヤ把持機構の変位についてｚ_t2a＝ｚ_t2b＝ｚ_t2cの場合には、以下の（５４）式となる。

ζ_IK＝ξ
θ_IK＝０
ｌ_d＝ｚ_t2a (54)

本実施形態では、ａワイヤ、ｂワイヤ及びｃワイヤの各ワイヤに独立して、上述した第１の実施形態や第２の実施形態のおける切り替え制御を適用する分散制御を行う。

図２５は、本発明の第５の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図２５に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－５」として記載する。この図２５において、図１、図１２及び図１８に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

連続体ロボットの制御システム１０－５は、図１と同様の構成として、連続体ロボット（Ｐ_n）１００、入力部２１１，２１２、切替制御部（Ｈ）２２０、運動学演算部（Kinematics）２３０、及び、逆運動学演算部（ＩＫ）２７０を有している。ここで、図２５に示す連続体ロボット（Ｐ_n）１００は、図２３に示す３次元立体駆動可能な連続体ロボット１００－５であるものとする。さらに、連続体ロボットの制御システム１０－５は、入力部２１４、ａワイヤ駆動制御機構（Ｋ_SWa）５１０、ｂワイヤ駆動制御機構（Ｋ_SWb）５２０、ｃワイヤ駆動制御機構（Ｋ_SWc）５３０を有している。

入力部２１１は、第１の実施形態と同様に、選択する駆動制御部を示すモード信号を切替制御部（Ｈ）２２０に対して入力する。入力部２１２は、基準軸１０１に対する湾曲可能部１１００の湾曲角度の目標指令値ｒｅｆ_θを運動学演算部（Kinematics）２３０等に対して入力する。入力部２１４は、基準軸（ｘ軸）に対する湾曲可能部１１００の旋回角度の目標指令値ｒｅｆ_ζを運動学演算部（Kinematics）２３０等に対して入力する。

ａワイヤ駆動制御機構（Ｋ_SWa）５１０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。このａワイヤ駆動制御機構（Ｋ_SWa）５１０は、ａワイヤ１０１１の駆動を制御する構成部である。

ｂワイヤ駆動制御機構（Ｋ_SWb）５２０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。このｂワイヤ駆動制御機構（Ｋ_SWb）５２０は、ｂワイヤ１０１２の駆動を制御する構成部である。

ｃワイヤ駆動制御機構（Ｋ_SWc）５３０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。このｃワイヤ駆動制御機構（Ｋ_SWc）５３０は、ｃワイヤ１０１３の駆動を制御する構成部である。

図２５に示すａワイヤ駆動制御機構（Ｋ_SWa）５１０、ｂワイヤ駆動制御機構（Ｋ_SWb）５２０、及び、ｃワイヤ駆動制御機構（Ｋ_SWc）５３０は、個別に設計してもよいし、同じ制御系であってもよい。なお、図２５では、フィードバックループは省略している。

運動学演算部（Kinematics）２３０から出力される各ワイヤ把持機構の目標変位ｒｅｆ_za、ｒｅｆ_zb及びｒｅｆ_zcは、（４９）式の湾曲角度θ₁及び旋回角度ζ₁に、湾曲角度の目標指令値ｒｅｆ_θ及び旋回角度の目標指令値ｒｅｆ_ζを代入することで得られる。

逆運動学演算部（ＩＫ）２７０は、ワイヤ把持機構の変位ｚ_t2a、ｚ_t2b及びｚ_t2cから湾曲角度θ_IK及び旋回角度ζ_IKを、（５１）式～（５４）式を用いて求める。

そして、以下の（５５）式に示す、運動学演算部（Kinematics）２３０に入力された湾曲角度の目標指令値ｒｅｆ_θと逆運動学演算部（ＩＫ）２７０から出力された湾曲角度θ_IKとの差分θ_errが、切替制御部（Ｈ）２２０に入力される。さらに、以下の（５６）式に示す、運動学演算部（Kinematics）２３０に入力された旋回角度の目標指令値ｒｅｆ_ζと逆運動学演算部（ＩＫ）２７０から出力された旋回角度ζ_IKとの差分ζ_errが、切替制御部（Ｈ）２２０に入力される。

θ_err＝ｒｅｆ_θ－θ_IK (55)
ζ_err＝ｒｅｆ_ζ－ζ_IK (56)

高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えでは、まず、入力部２１１から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示すモード信号が入力される。この場合、切替制御部（Ｈ）２２０は、上述した湾曲角度の差分θ_errが予め設定された閾値以下であって上述した旋回角度の差分ζ_errが予め設定された閾値以下である場合に、当該切り替えを行う。

一方、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２への切り替えでは、切替制御部（Ｈ）２２０は、上述した湾曲角度の差分θ_errの値及び上述した旋回角度の差分ζ_errの値に関わりなく、当該切り替えを行う。即ち、入力部２１１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を示すモード信号が入力された場合、切替制御部（Ｈ）２２０は、当該切り替えを行う。

第５の実施形態においても、第１の実施形態と同様に、湾曲可能部を湾曲させる駆動制御部としてバックドライバビリティが異なる複数の駆動制御部がある場合に、これらの切り替えを操作者の負担を軽減し且つ適切に行うことができる。

第５の実施形態における連続体ロボット１００－５は、複数のワイヤ１０１１～１０１３が駆動されることによって湾曲可能に構成された湾曲可能部１１００と、複数のワイヤ１０１１～１０１３のそれぞれを駆動する複数の駆動部である複数のアクチュエータ（不図示）を備えている。そして、第５の実施形態に係る連続体ロボットの制御システム１０では、複数のワイヤ１０１１～１０１３ごとに、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１及び高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を含む駆動制御機構（ａワイヤ駆動制御機構（Ｋ_SWa）５１０、ｂワイヤ駆動制御機構（Ｋ_SWb）５２０及びｃワイヤ駆動制御機構（Ｋ_SWc）５３０）を設けている。そして、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、入力部２１１から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示すモード信号が入力された場合には、上述した湾曲角度の差分θ_errが閾値以下であって上述した旋回角度の差分ζ_errが閾値以下である場合に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替える制御を行う。
かかる連続体ロボットの制御システム１０－５の構成によれば、当該切り替え時の目標値への急峻な追従が抑えられ、連続体ロボット自体や脆弱な対象物の損傷を防ぐことが実現できる。

また、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御が行われてる際に、入力部２１１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を示すモード信号が入力された場合には、上述した湾曲角度の差分θ_errの値及び上述した旋回角度の差分ζ_errの値に関わらず、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替える制御を行う。

（第６の実施形態）
次に、本発明の第６の実施形態について説明する。なお、以下に記載する第６の実施形態の説明では、上述した第１～第５の実施形態と共通する事項については説明を省略し、上述した第１～第５の実施形態と異なる事項について説明を行う。

第６の実施形態では、複数の湾曲可能部を有する３次元立体駆動可能な連続体ロボット１００に対する、制御系の切り替えを伴う先頭追従制御を行う。

図２６は、本発明の第６の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。以下、図２６に示す連続体ロボット１００を「連続体ロボット１００－６」として記載する。この図２６では、３つの湾曲可能部１１００－１～１１００－３が示されている。また、この図２６において、図２３に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

ここでは、第ｎの湾曲可能部１１００－ｎを駆動するａ～ｃワイヤの駆動変位をｌ_pna、ｌ_pnb及びｌ_pncとする。そして、ａ～ｃワイヤの駆動変位をｌ_pna、ｌ_pnb及びｌ_pncと第ｎの湾曲可能部１１００－ｎの遠位端の湾曲角度θ_n及び旋回角度ζ_nとの関係を求める。湾曲可能部１１００の数をｅとし、第ｎの湾曲可能部１１００－ｎを駆動するワイヤの位相角ξ_nを以下の（５７）式とする。

これより、第ｎの湾曲可能部１１００－ｎのａ～ｃワイヤの駆動変位ｌ_pna、ｌ_pnb及びｌ_pncは、以下の（５８）式となる。

上述した第５の実施形態と同様に、本実施形態においても、ワイヤの伸縮を考慮しないため、第ｎの湾曲可能部１１００－ｎのａ～ｃワイヤにおけるワイヤ把持機構の目標変位ｒｅｆ_zna、ｒｅｆ_znb及びｒｅｆ_zncは、以下の（５９）式となる。

ｒｅｆ_zna＝ｌ_pna、ｒｅｆ_znb＝ｌ_pnb、ｒｅｆ_znc＝ｌ_pnc (59)

これを用いて、上述した第５の実施形態と同様に、各ワイヤを切り替え制御系により駆動する。

図２７は、本発明の第６の実施形態に係る連続体ロボットの制御システム１０の概略構成の一例を示す図である。以下、図２７に示す連続体ロボットの制御システム１０を「連続体ロボットの制御システム１０－６」として記載する。この図２７において、図１、図１２、図１８及び図２５に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

連続体ロボットの制御システム１０－６は、図１と同様の構成として、連続体ロボット（Ｐ_n）１００、入力部２１１，２１２、切替制御部（Ｈ）２２０、運動学演算部（Kinematics）２３０、及び、逆運動学演算部（ＩＫ）２７０を有している。ここで、図２７に示す連続体ロボット（Ｐ_n）１００は、図２６に示す複数の湾曲可能部を有する３次元立体駆動可能な連続体ロボット１００－６であるものとする。さらに、連続体ロボットの制御システム１０－６は、入力部２１３，２１４、最遠位ａワイヤ駆動制御機構（Ｋ_SWLa）６１０、最遠位ｂワイヤ駆動制御機構（Ｋ_SWLb）６２０、最遠位ｃワイヤ駆動制御機構（Ｋ_SWLc）６３０、追従ａワイヤ駆動制御機構（Ｋ_SWFa）６４０、追従ｂワイヤ駆動制御機構（Ｋ_SWFb）６５０、追従ｃワイヤ駆動制御機構（Ｋ_SWFc）６６０を有している。

入力部２１１は、第１の実施形態と同様に、選択する駆動制御部を示すモード信号を切替制御部（Ｈ）２２０に対して入力する。入力部２１２は、基準軸１０１に対する最遠位湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θLを運動学演算部（Kinematics）２３０等に対して入力する。入力部２１４は、基準軸（ｘ軸）に対する最遠位湾曲可能部の旋回角度の目標指令値ｒｅｆ_ζLを運動学演算部（Kinematics）２３０等に対して入力する。入力部２１３は、ロボット基台１０４０の目標変位ｚ_brefを追従制御部（ＦＴＬ）３３０に対して入力する。

最遠位ａワイヤ駆動制御機構（Ｋ_SWLa）６１０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この最遠位ａワイヤ駆動制御機構（Ｋ_SWLa）６１０は、最遠位湾曲可能部のａワイヤの駆動を制御する構成部である。

最遠位ｂワイヤ駆動制御機構（Ｋ_SWLb）６２０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この最遠位ｂワイヤ駆動制御機構（Ｋ_SWLb）６２０は、最遠位湾曲可能部のｂワイヤの駆動を制御する構成部である。

最遠位ｃワイヤ駆動制御機構（Ｋ_SWLc）６３０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この最遠位ｃワイヤ駆動制御機構（Ｋ_SWLc）６３０は、最遠位湾曲可能部のｃワイヤの駆動を制御する構成部である。

追従ａワイヤ駆動制御機構（Ｋ_SWFa）６４０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この追従ａワイヤ駆動制御機構（Ｋ_SWFa）６４０は、追従湾曲可能部のａワイヤの駆動を制御する構成部である。

追従ｂワイヤ駆動制御機構（Ｋ_SWFb）６５０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この追従ｂワイヤ駆動制御機構（Ｋ_SWFb）６５０は、追従湾曲可能部のｂワイヤの駆動を制御する構成部である。

追従ｃワイヤ駆動制御機構（Ｋ_SWFc）６６０は、図１に示す遮断部２４１，２４２、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２、及び選択部２６０を含み構成されている。この追従ｃワイヤ駆動制御機構（Ｋ_SWFc）６６０は、追従湾曲可能部のｃワイヤの駆動を制御する構成部である。

最遠位ａワイヤ駆動制御機構（Ｋ_SWLa）６１０～追従ｃワイヤ駆動制御機構（Ｋ_SWFc）６６０は、個別に設計してもよいし、同じ制御系であってもよい。なお、図２７では、フィードバックループは省略している。

運動学演算部（Kinematics）２３０から出力される最遠位湾曲可能部を駆動する各ワイヤ把持機構の目標変位ｒｅｆ_zLa、ｒｅｆ_zLb及びｒｅｆ_zLcは、（５８）式の湾曲角度θ_n及び旋回角度ζ_nに、最遠位湾曲可能部の湾曲角度の目標指令値ｒｅｆ_θL及び旋回角度の目標指令値ｒｅｆ_ζLを代入することで得られる。

逆運動学演算部（ＩＫ）２７０に入力されるｚ_t2La、ｚ_t2Lb、ｚ_t2Lcは、それぞれ、最遠位湾曲可能部のａワイヤ、ｂワイヤ、ｃワイヤに対するワイヤ把持機構の変位である。また、追従湾曲可能部に関する信号名及びブロック名は、添え字のＬをＦに読み替えればよく、さらに、図２７では追従湾曲可能部を添え字のＦで代表させて図示している。なお、追従湾曲可能部が複数ある場合には、同じブロック構造が並列に並べばよい。

逆運動学演算部（ＩＫ）２７０は、（５１）式～（５４）式を用いて、最遠位湾曲可能部の湾曲角度θ_IKL及び旋回角度ζ_IKL、並びに、追従湾曲可能部の湾曲角度θ_IKF及び旋回角度ζ_IKFを求めることができる。ここで、追従制御部（ＦＴＬ）３３０は、湾曲方向に加えて、旋回方向の追従湾曲可能部の目標角度を、上述した第３の実施形態で示したアルゴリズムを用いて湾曲方向と同様に求める。

そして、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、入力部２１１から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１を示すモード信号が入力された場合には、図２７に示す湾曲角度の差分θ_errが閾値以下であって旋回角度の差分ζ_errが閾値以下である場合に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替える制御を行う。これにより、当該切り替え時の目標値への急峻な追従が抑えられ、連続体ロボット自体や脆弱な対象物の損傷を防ぐことが実現できる。

また、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御が行われてる際に、入力部２１１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２を示すモード信号が入力された場合には、図２７に示す湾曲角度の差分θ_errの値及び旋回角度の差分ζ_errの値に関わらず、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替える制御を行う。

（第７の実施形態）
次に、本発明の第７の実施形態について説明する。なお、以下に記載する第７の実施形態の説明では、上述した第１～第６の実施形態と共通する事項については説明を省略し、上述した第１～第６の実施形態と異なる事項について説明を行う。

上述した第１～第６の実施形態では、操作者が発生力Ｆを監視し、その発生力Ｆの値が所定範囲（±２Ｎ）を超えるか否かによって、入力部２１１からのモード信号を変更して制御系の切り替えを行うようにしていた。しかしながら、操作者の負担をより軽減する等のため、第７の実施形態では、上述した操作者の操作を連続体ロボットの制御システム１０が自動化して行う形態を採る。第７の実施形態では、例えば、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００において発生力Ｆを検出し、連続体ロボットの制御システム１０が、発生力Ｆの検出結果に基づいて入力部２１１からモード信号を発生させる形態を採りうる。以下に、第７の実施形態の適用例について説明する。

まず、第１～第４の実施形態に対して、第７の実施形態における自動化処理を適用した場合について説明する。
図３（ｃ）に示す連続体ロボット（Ｐ_n）１００は、ワイヤに印加された力である発生力Ｆの検出している。そして、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００による発生力Ｆの検出結果として所定範囲内（±２Ｎ以内）の力が検出された場合には、上述した湾曲可能部１１０の湾曲角度の差分θ_errが閾値以下である場合に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替える制御を行う。
また、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御が行われてる際に、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００による発生力Ｆの検出結果として所定範囲（±２Ｎ）を超える力が検出された場合に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替える制御を行う。

続いて、第５～第６の実施形態に対して、第７の実施形態における自動化処理を適用した場合について説明する。
図３（ｃ）に示す連続体ロボット（Ｐ_n）１００は、ワイヤに印加された力である発生力Ｆの検出している。そして、切替制御部（Ｈ）２２０は、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２による駆動制御が行われてる際に、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００による発生力Ｆの検出結果として所定範囲内（±２Ｎ以内）の力が検出された場合には、上述した湾曲可能部１１００の湾曲角度の差分θ_errが閾値以下であって且つ上述した湾曲可能部１１００の旋回角度の差分ζ_errが閾値以下ある場合に、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替える制御を行う。
また、切替制御部（Ｈ）２２０は、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１による駆動制御が行われてる際に、図３（ｃ）に示す連続体ロボット（Ｐ_n）１００による発生力Ｆの検出結果として所定範囲（±２Ｎ）を超える力が検出された場合に、低バックドライバビリティ駆動制御部（Ｋ_POS）２５１から高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２に切り替える制御を行う。

なお、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１に切り替えには、逆運動学演算部（ＩＫ）２７０により得られた角度と当該角度の目標指令値との差分が閾値以下であることを条件としているが、操作者が目標指令値を操作し、その操作に応じた差分が閾値以下であることを条件としてもよい。さらには、その操作を自動化してもよい。具体的には、例えば、モード信号と逆運動学演算部（ＩＫ）２７０により得られた角度と信号ｈ_OUTを入力とし、補助目標指令を出力とする機能ブロックを追加する。この補助目標指令は、目標指令に加算される。そして、この機能ブロックは、高バックドライバビリティ駆動制御部（Ｋ_BKD）２５２から低バックドライバビリティ駆動制御部（Ｋ_POS）２５１への切り替えが指令されると、補助目標指令を出力し、当該切り替えが完了すると、補助目標指令の出力を停止する。この補助目標指令は、逆運動学演算部（ＩＫ）２７０により得られた角度に追従するフィードバックループとするが、できるだけ低ゲインでフィードバックされることが望ましい。

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

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

１０：連続体ロボットの制御システム、１００：連続体ロボット（Ｐ_n）、２１１，２１２：入力部、２２０：切替制御部（Ｈ）、２３０：運動学演算部（Kinematics）、２４１，２４２：遮断部、２５１：低バックドライバビリティ駆動制御部（Ｋ_POS）、２５２：高バックドライバビリティ駆動制御部（Ｋ_BKD）、２６０：選択部、２７０：逆運動学演算部（ＩＫ）

Claims (14)

1. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、
   前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、
   前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、
   選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、
   を有し、
   前記第２の駆動制御部による駆動制御は、前記ワイヤの変位に関する目標値と検出値との間の差異を補償する制御と、前記ワイヤに印加される力に関する目標値と検出値との間の差異を補償する制御を含む
   ことを特徴とする連続体ロボットの制御システム。
2. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、
   前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、
   前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、
   選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、
   前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度を算出するための逆運動学演算を行う逆運動学演算部と、
   を有し、
   前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記モード信号として前記第１の駆動制御部を示す信号が入力された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う
   ことを特徴とする連続体ロボットの制御システム。
3. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、
   前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、
   前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、
   選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、
   前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度を算出するための逆運動学演算を行う逆運動学演算部と、
   を有し、
   前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記ワイヤに印加された力の検出結果として所定範囲内の力が検出された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う
   ことを特徴とする連続体ロボットの制御システム。
4. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、
   前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、
   前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、
   選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、
   前記運動学演算部は、前記湾曲可能部の湾曲角度と基準軸に対する前記湾曲可能部の旋回角度とから前記ワイヤの駆動量を算出するための運動学演算を行うものであり、
   前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度および前記湾曲可能部の旋回角度を算出するための逆運動学演算を行う逆運動学演算部と、
   を有し、
   前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記モード信号として前記第１の駆動制御部を示す信号が入力された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合であって前記運動学演算部に入力された前記湾曲可能部の旋回角度と前記逆運動学演算部から出力された前記湾曲可能部の旋回角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う
   ことを特徴とする連続体ロボットの制御システム。
5. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算部と、
   前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御部と、
   前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部であって、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御部と、
   選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御部と、
   前記運動学演算部は、前記湾曲可能部の湾曲角度と基準軸に対する前記湾曲可能部の旋回角度とから前記ワイヤの駆動量を算出するための運動学演算を行うものであり、
   前記運動学演算とは逆の演算であって前記ワイヤの駆動量から前記湾曲可能部の湾曲角度および前記湾曲可能部の旋回角度を算出するための逆運動学演算を行う逆運動学演算部と、
   を有し、
   前記切替制御部は、前記第２の駆動制御部による駆動制御が行われている際に、前記ワイヤに印加された力の検出結果として所定範囲内の力が検出された場合には、前記運動学演算部に入力された前記湾曲可能部の湾曲角度と前記逆運動学演算部から出力された前記湾曲可能部の湾曲角度との差分が閾値以下である場合であって前記運動学演算部に入力された前記湾曲可能部の旋回角度と前記逆運動学演算部から出力された前記湾曲可能部の旋回角度との差分が閾値以下である場合に、前記第２の駆動制御部から前記第１の駆動制御部に切り替える制御を行う
   ことを特徴とする連続体ロボットの制御システム。
6. 前記切替制御部は、前記第１の駆動制御部による駆動制御が行われている際に、前記モード信号として前記第２の駆動制御部を示す信号が入力された場合に、前記第１の駆動制御部から前記第２の駆動制御部に切り替える制御を行う
   ことを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
7. 前記切替制御部は、前記第１の駆動制御部による駆動制御が行われている際に、前記ワイヤに印加された力の検出結果として所定範囲を超える力が検出された場合に、前記第１の駆動制御部から前記第２の駆動制御部に切り替える制御を行う
   ことを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
8. 前記第２の駆動制御部は、前記ワイヤに印加される力の目標値と、前記ワイヤに印加された力の検出値と、の誤差を補償する機能を備える
   ことを特徴とする請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システム。
9. 前記切替制御部の制御に基づいて、前記第１の駆動制御部の出力または前記第２の駆動制御部の出力を選択する選択部を更に有する
   ことを特徴とする請求項１乃至８のいずれか１項に記載の連続体ロボットの制御システム。
10. 前記選択部は、前記選択する駆動制御部の出力を前記第１の駆動制御部の出力と前記第２の駆動制御部の出力とで切り替える場合に、前記第１の駆動制御部の出力と前記第２の駆動制御部の出力を連続的に補間する機能を備える
    ことを特徴とする請求項９に記載の連続体ロボットの制御システム。
11. 前記連続体ロボットは、基台を更に備えるとともに、複数の前記湾曲可能部であって前記基台から遠位にある遠位湾曲可能部および前記基台と前記遠位湾曲可能部との間にある追従湾曲可能部を備えており、
    前記遠位湾曲可能部および前記追従湾曲可能部ごとに、前記第１の駆動制御部および前記第２の駆動制御部を含む駆動制御機構が設けられている
    ことを特徴とする請求項１乃至１０のいずれか１項に記載の連続体ロボットの制御システム。
12. 前記運動学演算部に入力される前記遠位湾曲可能部の湾曲角度と前記基台の目標変位とに基づいて、前記追従湾曲可能部の湾曲角度を前記運動学演算部に出力する追従制御部を更に有する
    ことを特徴とする請求項１１に記載の連続体ロボットの制御システム。
13. 前記連続体ロボットは、複数の前記ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記複数のワイヤのそれぞれを駆動する複数の前記駆動部と、を備えており、
    前記複数のワイヤごとに、前記第１の駆動制御部および前記第２の駆動制御部を含む駆動制御機構が設けられている
    ことを特徴とする請求項１乃至１２のいずれか１項に記載の連続体ロボットの制御システム。
14. ワイヤが駆動されることによって湾曲可能に構成された湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御方法であって、
    運動学演算部が、基準軸に対する前記湾曲可能部の湾曲角度から前記ワイヤの駆動量を算出するための運動学演算を行う運動学演算ステップと、
    第１の駆動制御部が、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第１の駆動制御ステップと、
    前記湾曲可能部が外力を受けたときに前記第１の駆動制御部による駆動制御の場合よりも前記駆動部が逆方向に駆動されやすい第２の駆動制御部が、前記運動学演算の結果に基づいて前記駆動部による前記ワイヤの駆動量を制御する第２の駆動制御ステップと、
    選択する駆動制御部を示すモード信号または前記ワイヤに印加された力の検出結果に応じて、前記第１の駆動制御部と前記第２の駆動制御部とを切り替える制御を行う切替制御ステップと、
    を有し、
    前記第２の駆動制御部による駆動制御は、前記ワイヤの変位に関する目標値と検出値との間の差異を補償する制御と、前記ワイヤに印加される力に関する目標値と検出値との間の差異を補償する制御を含む
    ことを特徴とする連続体ロボットの制御方法。