JP2023088151A - 連続体ロボット制御システム、連続体ロボット制御方法及びプログラム - Google Patents

Abstract

【課題】連続体ロボットが前進した後に後進する際に、連続体ロボットの破損を防止することが可能な仕組みを提供する。【解決手段】連続体ロボットを前進させて最遠位湾曲部（先頭湾曲部）を管の内部に挿入した後に、連続体ロボットを後進させて最遠位湾曲部（先頭湾曲部）を管から抜き出す際に、基台部の位置が位置ｃ（第３の位置）と位置ｅ（第１の位置）との間で、最遠位湾曲部（先頭湾曲部）が基準軸に対して湾曲角度ｂ（第２の湾曲角度）と湾曲角度ｅ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した状態となる制御を行う。【選択図】図８

Description

本発明は、連続体ロボットの制御を行う連続体ロボット制御システム及び連続体ロボット制御方法、並びに、当該連続体ロボット制御システムとしてコンピュータを機能させるためのプログラムに関するものである。

連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲部からなり、その湾曲部を変形させることによって形状が制御される。この連続体ロボットは、剛体リンクによって構成される剛体リンクロボットに対して、主に２つの優位性を持つ。１つ目の優位性としては、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、連続体ロボットは、曲線に沿って移動可能である。２つ目の優位性としては、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かして、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。

特許文献１には、内視鏡として用いられる連続体ロボットを管腔に侵入するための制御手法が記載されている。具体的に、特許文献１では、隣接する湾曲部の全ての組において、内視鏡の基台部の前進に伴って前方の湾曲部の湾曲形状を後続の湾曲部の湾曲形状にするように制御することで、内視鏡の形状を連続的に伝播させている。

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

上述した特許文献１では、内視鏡として用いられる連続体ロボットの基台部が湾曲部の長さを前進するごとに、前方の湾曲部の湾曲形状を後続の湾曲部に伝播させる制御に係る技術を記載している。しかしながら、特許文献１に記載の技術では、連続体ロボットが前進した後に後進する際には、その姿勢制御なしでは管腔の内壁に連続体ロボットが接触して、連続体ロボットが破損してしまうおそれがあった。

本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットが前進した後に後進する際に、連続体ロボットの破損を防止することが可能な仕組みを提供することを目的とする。

本発明の連続体ロボット制御システムは、基台部と、遠位用の線状部材が駆動されることによって湾曲する遠位湾曲部と、前記遠位用の線状部材を駆動する駆動部と、前記遠位湾曲部と前記基台部との間に設けられた連結部と、を有する連続体ロボットの制御を行う連続体ロボット制御システムであって、前記連続体ロボットを前進および後進させる移動手段と、前記連続体ロボットの前記基台部が第１の位置にあって前記遠位湾曲部が基準軸に対して第１の湾曲角度に湾曲した第１の状態、前記基台部が前記第１の位置よりも前記前進した第２の位置にあって前記遠位湾曲部が前記基準軸に対して前記第１の湾曲角度とは異なる第２の湾曲角度に湾曲した第２の状態、および、前記基台部が前記第２の位置よりも前記前進した第３の位置にある第３の状態の順に遷移することで、前記遠位湾曲部を管の内部に挿入した後に、前記連続体ロボットを前記後進させて前記遠位湾曲部を前記管から抜き出す際に、前記基台部の位置が前記第３の位置と前記第１の位置との間で、前記遠位湾曲部が前記基準軸に対して前記第２の湾曲角度と前記第１の湾曲角度との間の第３の湾曲角度に湾曲した第４の状態をとるように制御する制御手段と、を有する。
また、本発明は、上述した連続体ロボット制御システムによる連続体ロボット制御方法、及び、上述した連続体ロボット制御システムの各手段としてコンピュータを機能させるためのプログラムを含む。

本発明によれば、連続体ロボットが前進した後に後進する際に、連続体ロボットの破損を防止することが可能となる。

本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 図１に示す連続体ロボットの概略構成のうち、１つの湾曲部に関する詳細な概略構成の一例を示す図である。 図２に示す３つのワイヤのｘｙ平面における配置例を示す図である。 本発明の第１の実施形態に係る連続体ロボット制御システムの概略構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 図５に示す第ｎの湾曲部の部分を抽出した図である。 図４の制御装置によって行われる連続体ロボットの先頭追従制御及び後尾追従制御を説明するための図である。 本発明の第１の実施形態を示し、図４の制御装置に記憶される参照テーブルとして、連続体ロボットの先頭追従制御及び後尾追従制御の参照テーブルの一例を示す図である。 本発明の第１の実施形態を示し、図４の制御装置に記憶される参照テーブルとして、連続体ロボットの後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。 本発明の第１の実施形態を示し、図４の制御装置に記憶される参照テーブルとして、連続体ロボットの後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットにおいて、先頭追従制御の応答に係るシミュレーションの結果を示す図である。 本発明の第１の実施形態に係る連続体ロボットにおいて、図８（ｂ）に示す後尾追従制御の補間法１の応答に係るシミュレーションの結果を示す図である。 本発明の第１の実施形態に係る連続体ロボットにおいて、図９（ａ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。 本発明の第１の実施形態に係る連続体ロボットにおいて、図９（ｂ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。 本発明の第１の実施形態に係る連続体ロボットにおいて、図１０（ａ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。 本発明の第２の実施形態を示し、図４の制御装置に記憶される参照テーブルとして、連続体ロボットの後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。 本発明の第３の実施形態を示し、図４の制御装置に記憶される参照テーブルとして、連続体ロボットの後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。 本発明の第４の実施形態に係る連続体ロボット制御システムの概略構成の一例を示す図である。

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

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

図１は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。図１には、連続体ロボット１００の一構成として、基台部１４０及び複数の湾曲部（「湾曲区間」としてもよい）１７０が図示されている。具体的に、図１に示す例では、複数の湾曲部１７０として、基台部１４０の側から、湾曲部１７０－（ｎ－２）、湾曲部１７０－（ｎ－１）及び湾曲部１７０－ｎのｎ個の湾曲部が図示されている。また、図１では、基台部１４０の上面１４１の所定位置（例えば中心の位置）に原点Ｏをとり、連続体ロボット１００の進行方向（例えば前進する方向）をｚ方向とし、このｚ方向と直交する方向であって相互に直交する方向をｘ方向及びｙ方向とした、ｘｙｚ座標系を図示している。さらに、図１では、連続体ロボット１００を前進させて挿入し且つ連続体ロボット１００を後進させて抜き出すための経路に相当する管２０を図示している。この管２０は、連続体ロボット１００を医療分野で用いる場合には、例えば消化管や気管などを適用することができ、また、連続体ロボット１００を工業分野で用いる場合には、例えば配管などを適用することができるが、これらに限定されるものではない。

図１に示す複数の湾曲部１７０における各湾曲部は、当該各湾曲部ごとに備えられた線状部材であるワイヤが駆動されることによって湾曲する湾曲部である。図１に示す湾曲部１７０－ｎは、湾曲部１７０－（ｎ－１）及び湾曲部１７０－（ｎ－２）よりも基台部１４０から遠位にある遠位湾曲部（最遠位湾曲部）であり、また、連続体ロボット１００が前進する方向（＋ｚ方向）において先頭湾曲部である。図１に示す湾曲部１７０－（ｎ－１）は、連続体ロボット１００が前進する方向（＋ｚ方向）において、先頭湾曲部である湾曲部１７０－ｎに後続する第１の後続湾曲部である。図１に示す湾曲部１７０－（ｎ－２）は、連続体ロボット１００が前進する方向（＋ｚ方向）において、湾曲部１７０－ｎ及び湾曲部１７０－（ｎ－１）に後続する第２の後続湾曲部である。また、図１において、湾曲部１７０－（ｎ－１）及び湾曲部１７０－（ｎ－２）は、遠位湾曲部（最遠位湾曲部）である湾曲部１７０－ｎと基台部１４０との間に設けられた連結部に相当する。

また、図１では、ｚ方向と平行な基準軸１０１に対する湾曲部１７０－ｎの遠位端の湾曲角度（単に「湾曲部１７０－ｎの湾曲角度」とする）をθ_nで示している。また、図１では、ｚ方向と平行な基準軸１０１に対する湾曲部１７０－（ｎ－１）の遠位端の湾曲角度（単に「湾曲部１７０－（ｎ－１）の湾曲角度」とする）をθ_n-1で示している。さらに、図１では、ｚ方向と平行な基準軸１０１に対する湾曲部１７０－（ｎ－２）の遠位端の湾曲角度（単に「湾曲部１７０－（ｎ－２）の湾曲角度」とする）をθ_n-2で示している。

基台部１４０は、複数の湾曲部１７０を支える構成部であり、その内部には、湾曲部１７０－ｎ～１７０－（ｎ－２）のそれぞれのワイヤを駆動させるための駆動部であるアクチュエータ（図１では不図示）が設けられている。

連続体ロボット１００は、上述した複数の湾曲部１７０による湾曲動作に加えて、ｚ方向に前進（＋ｚ方向）及び後進（－ｚ方向）の移動が可能となっている。この際、図１では、連続体ロボット１００のｚ方向の移動量を示す指標として基台部１４０の変位ｚ_bを図示している。

図２は、図１に示す連続体ロボット１００の概略構成のうち、１つの湾曲部１７０に関する詳細な概略構成の一例を示す図である。具体的に、図２は、図１に示す複数の湾曲部１７０のうち、基台部１４０から最も近い位置にある近位端の湾曲部１７０－（ｎ－２）に関する詳細な概略構成を示している。この図２において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。なお、図２では、ｚ方向と平行な基準軸１０１に対する湾曲部１７０の湾曲角度をθ₁で示し、また、湾曲部１７０の旋回角度をζ₁で示し、湾曲部１７０の曲率半径（図２では、原点Ｏと点ｗ₁とを結ぶ線分に相当する）をρ₁で示している。

なお、図１においては、湾曲部１７０－ｎの旋回角度をζ_nで示し、湾曲部１７０－（ｎ－１）の旋回角度をζ_n-1で示し、湾曲部１７０－（ｎ－２）の旋回角度をζ₁で示している。

図２に示すように、連続体ロボット１００は、湾曲部１７０の遠位端１６０の接続部１２１～１２３に、それぞれ、線状部材であるワイヤ１１１～１１３が接続されている。このワイヤ１１１～１１３が基台部１４０の内部に設置されているそれぞれのアクチュエータ１３１～１３３で押し引きされることによって、姿勢（湾曲形状）が制御される。ここで、アクチュエータ１３１は、ワイヤ１１１を駆動させるための駆動部であり、また、アクチュエータ１３２は、ワイヤ１１２を駆動させるための駆動部であり、さらに、アクチュエータ１３３は、ワイヤ１１３を駆動させるための駆動部である。

また、図２に示すように、連続体ロボット１００は、湾曲部１７０において、線状部材であるワイヤ１１１～１１３を案内するための部材であるワイヤガイド１６１～１６４を有する。ワイヤガイド１６１～１６４は、複数の部材を離散的に配置する方法の他に、蛇腹状やメッシュ状などの連続体部材を用いてもよい。ワイヤ１１１は、ワイヤガイド１６１～１６４に、それぞれ固定部１５０～１５３において固定される。また、図２において、原点Ｏを通る連続体ロボット１００の中心軸を破線で示している。

また、本実施形態では、ワイヤ１１１～１１３を、ｘｙ平面内において反時計回りに、ａワイヤ、ｂワイヤ及びｃワイヤと呼ぶ。具体的に、図２に示す例では、ワイヤ１１１がａワイヤに相当し、湾曲部１７０のアクチュエータ１３１の押し引きによるワイヤ１１１の駆動変位をｌ_p1aとして図示している。また、図２に示す例では、ワイヤ１１２がｂワイヤに相当し、湾曲部１７０のアクチュエータ１３２の押し引きによるワイヤ１１２の駆動変位をｌ_p1bとして図示している。さらに、図２に示す例では、ワイヤ１１３がｃワイヤに相当し、湾曲部１７０のアクチュエータ１３３の押し引きによるワイヤ１１３の駆動変位をｌ_p1cとして図示している。ここで、一般化して、第ｎの湾曲部（ｎは、正の整数）を駆動するワイヤの駆動変位をｌ_pna，ｌ_pnb，ｌ_pncとする。

なお、図２に示す例では、図１に示す湾曲部１７０－（ｎ－２）に相当する湾曲部１７０に関する詳細な概略構成のみを図示してその説明を行った。この点、図１に示す湾曲部１７０－（ｎ－１）及び湾曲部１７０－ｎについても、図１に示す湾曲部１７０－（ｎ－２）に相当する図２の湾曲部１７０に関する詳細な概略構成と同様の構成を備えている。即ち、湾曲部１７０－（ｎ－１）及び１７０－ｎは、それぞれが、ワイヤ１１１～１１３に相当するワイヤや、アクチュエータ１３１～１３３に相当するアクチュエータ、遠位端１６０及びワイヤガイド１６１～１６４に相当する遠位端及びワイヤガイドを備える。例えば、図１の最遠位湾曲部（先頭湾曲部）である湾曲部１７０－ｎは、図２のワイヤ１１１～１１３に相当する最遠位用の線状部材であるワイヤを備えており、当該ワイヤが駆動部であるアクチュエータによって駆動されることによって湾曲する。また、例えば、図１の第１の後続湾曲部である湾曲部１７０－（ｎ－１）は、図２のワイヤ１１１～１１３に相当する第１の後続用の線状部材であるワイヤを備えており、当該ワイヤが駆動部であるアクチュエータによって駆動されることによって湾曲する。そして、図１の第２の後続湾曲部である湾曲部１７０－（ｎ－２）は、第２の後続用の線状部材である図２のワイヤ１１１～１１３を備えており、ワイヤ１１１～１１３が駆動部であるアクチュエータ１３１～１３３によって駆動されることによって湾曲する。即ち、図１に示す連続体ロボット１００は、最遠位用の線状部材であるワイヤと、第１の後続用の線状部材であるワイヤと、第２の後続用の線状部材であるワイヤとを、それぞれ、独立して駆動する駆動部であるアクチュエータを備えて構成されている。

図３は、図２に示す３つのワイヤ１１１～１１３（ａワイヤ～ｃワイヤ）のｘｙ平面における配置例を示す図である。図３に示すように、図２に示す３つのワイヤ１１１～１１３（ａワイヤ～ｃワイヤ）は、一片の長さをｒ_sとする正三角形の頂点に配置されており、また、図３に示す位相角ξ_nは、第ｎの湾曲部を駆動するワイヤの配置を決定する角度である。

図４は、本発明の第１の実施形態に係る連続体ロボット制御システム１０－１の概略構成の一例を示す図である。この連続体ロボット制御システム１０－１は、図１及び図２に示す連続体ロボット１００の制御を行うシステムである。この連続体ロボット制御システム１０－１は、図４に示すように、連続体ロボット１００、制御装置２００、並びに、各種の入力装置３１０、３３０及び３４０を有して構成されている。

入力装置３１０は、図１に示す例では、最遠位湾曲部（先頭湾曲部）である湾曲部１７０－ｎの目標湾曲角度θ_Lを制御装置２００に入力する装置である。

入力装置３３０は、連続体ロボット１００のｚ方向の移動量を示す基台部１４０の変位ｚ_bを制御装置２００に入力するとともに、連続体ロボット１００の基台部１４０を変位ｚ_bだけｚ方向に移動させるべく制御を行う装置である。ここで、連続体ロボット１００の基台部１４０を変位ｚ_bだけｚ方向に移動させるべく制御を行う入力装置３３０は、連続体ロボット１００を変位ｚ_bだけｚ方向に移動（＋ｚ方向への移動は前進、－ｚ方向への移動は後進）させる「移動手段」を構成する。

入力装置３４０は、各種の情報を制御装置２００に入力する装置である。具体的に、入力装置３４０は、例えば、図１に示す例の場合には複数の湾曲部１７０－ｎ～１７０－（ｎ－２）の長さｌの情報や、使用する参照テーブル２１１１の指示情報等を、制御装置２００に入力する。

制御装置２００は、図４に示すように、角度算出部２１０、切替部２２０、及び、運動学演算部（Kinematics）２３０を有して構成されている。

角度算出部２１０には、入力装置３１０から最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_L、入力装置３３０から基台部１４０の変位ｚ_b、入力装置３４０から複数の湾曲部１７０の長さｌの情報が入力される。そして、角度算出部２１０は、入力された情報に基づいて、後続湾曲部（例えば、上述した第１の後続湾曲部）の目標湾曲角度θ_Fの算出や、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_Lの変更に伴う変更後の後続湾曲部の目標湾曲角度θ_F'を算出する。さらに、角度算出部２１０は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_Lや後続湾曲部の目標湾曲角度θ_Fを記憶し、連続体ロボット１００の基台部１４０が後進するときの最遠位湾曲部の目標湾曲角度θ_L'を算出する。

角度算出部２１０は、図４に示すように、マルチプレクサ（ＭＵＸ）、記憶部２１１、参照テーブル書換部２１２、及び、情報入力部２１３を有して構成されている。マルチプレクサ（ＭＵＸ）は、外部の装置から複数の入力情報を受け、それらを選択したりまとめたりして１つの情報（信号）として出力する構成部である。記憶部２１１は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_L及び後続湾曲部の目標湾曲角度θ_Fと基台部１４０の変位ｚ_bとの関係を示す異なる複数の参照テーブル２１１１や、角度算出部２１０において処理に必要な各種の情報を記憶している。情報入力部２１３は、入力装置３４０から入力された複数の湾曲部１７０の長さｌの情報や使用する参照テーブル２１１１の指示情報等を、参照テーブル書換部２１２に対して入力する。参照テーブル書換部２１２は、情報入力部２１３から入力された情報に基づいて、記憶部２１１に記憶されている複数の参照テーブル２１１１の中から使用する１つの参照テーブル２１１１を選択する。そして、参照テーブル書換部２１２は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_L、後続湾曲部の目標湾曲角度θ_F及び基台部１４０の変位ｚ_bの変更に応じて、当該選択した参照テーブル２１１１を書き換える。

切替部２２０は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度として、入力装置３１０から入力された目標湾曲角度θ_Lと、角度算出部２１０により算出された目標湾曲角度θ_L'とを、基台部１４０の変位ｚ_bの進行方向に応じて切り替える。具体的に、本実施形態では、切替部２２０は、基台部１４０の変位ｚ_bの進行方向が前進の方向である場合には、入力装置３１０から入力された目標湾曲角度θ_Lを選択する切り替えを行う。また、切替部２２０は、基台部１４０の変位ｚ_bの進行方向が後進の方向である場合には、角度算出部２１０により算出された目標湾曲角度θ_L'を選択する切り替えを行う。

運動学演算部２３０には、切替部２２０から出力された最遠位湾曲部（先頭湾曲部）の目標湾曲角度（θ_Lまたはθ_L'）と、角度算出部２１０で算出された後続湾曲部の目標湾曲角度θ_F'の情報が入力される。そして、運動学演算部２３０は、入力された情報に基づいて、駆動部である各アクチュエータで最遠位湾曲部（先頭湾曲部）及び後続湾曲部のワイヤを駆動させる際の駆動変位を演算する。図４では、運動学演算部２３０の演算により得られた最遠位湾曲部（先頭湾曲部）のワイヤの駆動変位と後続湾曲部のワイヤの駆動変位をまとめて、ワイヤの駆動変位ｌ_pとして図示している。そして、本実施形態に係る連続体ロボット１００は、それぞれのワイヤの駆動変位ｌ_pに応じて、最遠位湾曲部（先頭湾曲部）及び後続湾曲部の湾曲制御が行われることになる。

本実施形態では、すべての位相角（図３）をξ_n＝０とし、まず、ｘｚ平面における先頭追従制御について説明する。

１）モデリング
本章では、連続体ロボット１００のｘｚ平面における運動学を導出する。記号の定義を以下に示す。
ｌ_n：第ｎの湾曲部の（アーム筐体の）長さ
ｒ_n：第ｎの湾曲部のワイヤガイドを通るワイヤからワイヤガイドの中心までの距離
ｅ：連続体ロボット１００の湾曲部の数
θ_n：第ｎの湾曲部の（遠位端の）湾曲角度
ρ_n：第ｎの湾曲部の曲率半径
θ_refn：第ｎの湾曲部の（遠位端の）目標湾曲角度
ｌ_pn：第ｎの湾曲部のワイヤの駆動変位
ｘ_tn，ｚ_tn：第ｎの湾曲部の遠位端の座標
ｚ_b：基台部１４０の変位

図５は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。この図５において、図１及び図２に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。そして、本実施形態では、以下のような仮定をし、図５に示す連続体ロボット１００の湾曲部１７０の数をｅとする連続体ロボット１００の運動学を導出する。ここで、図５に示す連続体ロボット１００では、第ｅの湾曲部１７０－ｅは、基台部１４０から最遠位にある最遠位湾曲部であり、また、連続体ロボット１００が前進する方向（＋ｚ方向）において先頭湾曲部である。
１．ワイヤは、ｘｚ面内のみに変形する。
２．各湾曲部において、ワイヤは曲率一定に変形する。
３．ワイヤのねじり変形は考慮しない。
４．ワイヤは、長手方向に変形しない。

まず、図５に示す第１の湾曲部１７０－１のみを考える。
ａワイヤを駆動し、ｂワイヤ及びｃワイヤを固定するとき、ワイヤの駆動量ｌ_p1と第１の湾曲部１７０－１の湾曲角度θ₁との関係は、以下の（１）式となる。

次に、図５に示す第ｎの湾曲部１７０－ｎのワイヤの駆動変位ｌ_pnと、その遠位端の湾曲角度θ_nとの関係を導出する。ただし、ここでは、ｎは２以上の整数とする。第ｎの湾曲部１７０－ｎの相対湾曲角度θ^~ _nを以下の（２）式に示すように定義する。
θ~_n＝θ_n－θ_n-1 ・・・（２）

図６は、図５に示す第ｎの湾曲部１７０－ｎの部分を抽出した図である。この図６に示すように、第ｎの湾曲部１７０－ｎの原点Ｏを（ｘ_tn-1，ｚ_tn-1）とし、θ_n-1方向及び
その直交方向からなる相対座標系ｘ_n－ｚ_nをとる。そうすると、相対座標系ｘ_n－ｚ_n
におけるワイヤの駆動変位ｌ~_pnと第ｎの湾曲部１７０－ｎの相対湾曲角度θ~_nとの関係は、以下の（３）式となる。

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

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

次に、第ｎの湾曲部１７０－ｎの（遠位端の）湾曲角度θ_nとその遠位端の座標との関係について説明する。

まず、第１の湾曲部１７０－１の（遠位端の）湾曲角度θ₁とその遠位端の座標（ｘ_t1，ｚ_t1）との関係は、以下の（５）式及び（６）式となる。

続いて、第ｎの湾曲部１７０－ｎの（遠位端の）湾曲角度θ_nとその遠位端の座標との関係を導出する。ただし、ｎは、２以上とする。相対座標系ｘ_n－ｚ_nにおける第ｎの湾曲部１７０－ｎの遠位端の座標（ｘ~_tn，z~_tn）は、以下の（７）式及び（８）式となる。

これにより、絶対座標系における第ｎの湾曲部１７０－ｎの遠位端の座標（ｘ_tn，z_tn）は、回転変換行列を用いて、以下の（９）式となる。

次に、制御装置２００によって行われる連続体ロボット１００の先頭追従制御及び後尾追従制御について説明する。図７は、図４の制御装置２００によって行われる連続体ロボット１００の先頭追従制御及び後尾追従制御を説明するための図である。具体的に、図７（ａ）は、先頭追従制御を説明するための図であり、図７（ｂ）は、後尾追従制御を説明するための図である。また、図７（ａ）及び図７（ｂ）の説明では、図５に示す連続体ロボット１００を想定した説明を行う。即ち、図７（ａ）及び図７（ｂ）の説明では、第ｅの湾曲部１７０－ｅが最遠位湾曲部（先頭湾曲部）であり、第（ｅ－１）の湾曲部１７０－（ｅ－１）が第ｅの湾曲部１７０－ｅに後続する後続湾曲部とした説明を行う。このため、図７（ａ）及び図７（ｂ）において、図５に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

２）先頭追従制御
本章では、先頭追従制御系について説明する。
ここで、先頭追従制御とは、図７（ａ）に示すように、最遠位湾曲部（先頭湾曲部）である第ｅの湾曲部１７０－ｅが通る点線で示す経路と同じ経路を後続湾曲部である第（ｅ－１）の湾曲部１７０－（ｅ－１）等が通るように制御する方法である。これにより、図７（ａ）の状態７１１～７１５に示すように連続体ロボット１００を順次前進させる際に、連続体ロボット１００は、管２０の内部の空間をすり抜けるように前進することができる。この先頭追従制御は、予め経路を定められていることは必須ではなく、例えば、最遠位湾曲部（先頭湾曲部）である第ｅの湾曲部１７０－ｅの湾曲角度θ_eを後続湾曲部に湾曲部の長さｌをもって連続的に伝播させてもよい。この方法を用いると、操縦者は、入力装置３１０及び入力装置３３０を介して最遠位湾曲部（先頭湾曲部）の湾曲角度と基台部１４０のｚ方向の移動量に対してのみ指令を与えることで、連続体ロボット１００を実時間で先頭追従制御することができる。

図８は、本発明の第１の実施形態を示し、図４の制御装置２００に記憶される参照テーブル２１１１として、連続体ロボット１００の先頭追従制御及び後尾追従制御の参照テーブルの一例を示す図である。

具体的に、図８（ａ）には、連続体ロボット１００の先頭追従制御の参照テーブル２１１１が示されている。具体的に、図８（ａ）に示す参照テーブル２１１１は、横軸に上述した基台部１４０の変位ｚ_bをとり、縦軸に最遠位湾曲部（先頭湾曲部）及び後続湾曲部の湾曲角度θをとったグラフとして示されている。また、図８（ａ）に示す参照テーブル２１１１において、横軸に示される基台部１４０の変位ｚ_bは、右の矢印に示す方向が、連続体ロボット１００の基台部１４０が前進する方向になっている。また、図８（ａ）に示す原点Ｏの位置（原点位置）は、例えば、図５に示す原点Ｏの位置（原点位置）に対応している。さらに、図８（ａ）の縦軸の湾曲角度θは、図７（ａ）に示す最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が「Ｌｅａｄｅｒ」として示され、後続湾曲部１７０－（ｅ－１）の湾曲角度が「Ｆｏｌｌｏｗｅｒ」として示されているものとする。そして、この場合、図８（ａ）の横軸に示される基台部１４０の変位ｚ_bにおいて、位置ｅと位置ａとの距離は最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌに相当し、位置ａと位置ｃとの距離は後続湾曲部１７０－（ｅ－１）の長さｌに相当している。

図８（ａ）では、破線で入力装置３１０から入力される最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示し、太破線で後続湾曲部１７０－（ｅ－１）の湾曲角度を示す。図８（ａ）では、基台部１４０の変位ｚ_bにおける位置ａにおいて、入力装置３１０から最遠位湾曲部（先頭湾曲部）１７０－ｅの目標湾曲角度θ_Lとして湾曲角度ａ→ｂが入力された場合を示している。この場合、後続湾曲部１７０－（ｅ－１）の湾曲角度は、基台部１４０の変位ｚ_bにおける位置ａから後続湾曲部１７０－（ｅ－１）の長さｌの距離にある位置ｃにおいて、湾曲角度ｃ→ｄとなるように角度算出部２１０で自動生成されうる。そして、後続湾曲部１７０－（ｅ－１）の湾曲角度は、制御装置２００の記憶部２１１に収納され、基台部１４０の変位ｚ_bに応じて読み出される。湾曲部１７０の数ｅが３以上の場合には、上述した後続湾曲部を最遠位湾曲部（先頭湾曲部）と置き換えて、この処理を連続的に行えば全ての湾曲部１７０の湾曲角度を求めることができる。

しかしながら、上述した図８（ａ）の説明では、基台部１４０の変位ｚ_bが位置ａと位置ｃとの間にあるときには、後続湾曲部１７０－（ｅ－１）の湾曲角度が変化せず、位置ｃで湾曲角度ｃ→ｄとに立ち上がる。このため、連続体ロボット１００は、急峻な挙動を示すことになる。そこで、本実施形態では、基台部１４０の変位ｚ_bが位置ａと位置ｃとの間にあるときには、後続湾曲部１７０－（ｅ－１）の湾曲角度について、湾曲角度ａと湾曲角度ｄとを結ぶように補間する。具体的に、本実施形態では、制御装置２００の角度算出部２１０は、まず、図８（ａ）において斜線で示す直角三角形として、後続湾曲部１７０－（ｅ－１）の長さｌを線分ａｃに一致させた底辺とし、距離ａｂを高さとする直角三角形を生成する。次いで、制御装置２００の角度算出部２１０は、位置ａから位置ｃまでの基台部１４０の変位ｚ_bの各位置において、基台部１４０の変位ｚ_bの座標軸と直交する直線と生成した直角三角形の斜辺との交点を求める。そして、制御装置２００の角度算出部２１０は、求めた交点から生成した直角三角形の底辺までの長さを、太破線で示す後続湾曲部１７０－（ｅ－１）の湾曲角度に加算して、補間後の後続湾曲部１７０－（ｅ－１）の湾曲角度を生成する。ここで、補間後の後続湾曲部１７０－（ｅ－１）の湾曲角度は、図８（ａ）において太実線で示されている。

３）後尾追従制御
３．１）補間法１
続いて、本章では、後尾追従制御について説明する。まず、第１節として、後尾追従制御の補間法１について説明する。

本実施形態では、図７（ｂ）に示す後尾追従制御は、連続体ロボット１００を点線で示す経路に近接させながら連続体ロボット１００を後進させて基台部１４０を原点Ｏに復帰させる制御方法である。これにより、図７（ｂ）の状態７２１～７２５に示すように連続体ロボット１００を順次後進させる際に、連続体ロボット１００は、管２０の内部の空間をすり抜けるように後進することができる。この際、基台部１４０に近位の湾曲部１７０の目標湾曲角度は、図８（ａ）において太実線で示した補間後の後続湾曲部１７０－（ｅ－１）の湾曲角度を、基台部１４０の変位ｚ_bに応じて記憶部２１１から読み出せばよい。

最遠位湾曲部（先頭湾曲部）１７０－ｅの目標湾曲角度は、まず、先頭追従制御において入力装置３１０から入力された最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を記憶部２１１に収納する。このとき、基台部１４０に近位の後続湾曲部１７０－（ｅ－１）の湾曲角度と同様に、第２章で説明した手法を用いて補間して記憶部２１１に収納すればよい。また、本実施形態では、基台部１４０の変位ｚ_bにおいて、原点Ｏの位置から最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌよりも遠位の位置ａで、入力装置３１０から最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度ａ→ｂの入力がなされるものとする。

図８（ｂ）には、連続体ロボット１００の後尾追従制御（先頭追従制御も含みうる）の参照テーブル２１１１が示されている。この図８（ｂ）において、図８（ａ）に示す連続体ロボット１００の先頭追従制御に係る要素と同様の要素については同じ符号を付しており、その詳細な説明は省略する。具体的に、図８（ｂ）では、図７（ａ）及び図８（ａ）を用いて説明した連続体ロボット１００の前進に係る先頭追従制御によって、基台部１４０は、少なくとも位置ｃまでは前進しているものとする。そして、図８（ｂ）では、基台部１４０が少なくとも位置ｃまで前進している状態から、連続体ロボット１００に対して後尾追従制御がなされるものとする。

図８（ｂ）において、最遠位湾曲部（先頭湾曲部）の後尾追従制御がなされる場合、制御装置２００の角度算出部２１０は、まず、最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌを線分ｅｃに一致させた底辺とし、距離ａｂを高さとする直角三角形を生成する。次いで、制御装置２００の角度算出部２１０は、位置ｃから位置ｅまでの基台部１４０の変位ｚ_bの各位置において、基台部１４０の変位ｚ_bの座標軸と直交する直線と生成した直角三角形の斜辺との交点を求める。そして、制御装置２００の角度算出部２１０は、求めた交点から生成した直角三角形の底辺までの長さを、破線で示す最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度に加算して、補間後の最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を生成する。ここで、補間後の最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度は、図８（ｂ）において実線で示されている。

具体的に、図８に示す連続体ロボット１００の先頭追従制御及び後尾追従制御の補間法１では、制御装置２００は、以下の制御を行うようにしている。
連続体ロボット１００の前進に係る先頭追従制御では、制御装置２００は、まず、基台部１４０が位置ｅ（第１の位置）にあって最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｅ（第１の湾曲角度）に湾曲した第１の状態となる制御を行う。続いて、制御装置２００は、基台部が位置ｅ（第１の位置）よりも前進した位置ａ（第２の位置）にあって最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｅとは異なる湾曲角度ｂ（第２の湾曲角度）に湾曲した第２の状態となる制御を行う。続いて、制御装置２００は、基台部１４０が位置ａ（第２の位置）よりも前進した位置ｃ（第３の位置）にある第３の状態となる制御を行う。即ち、連続体ロボット１００の前進に係る先頭追従制御では、制御装置２００は、上述した第１の状態、第２の状態及び第３の状態の順に遷移することで、少なくとも最遠位湾曲部（先頭湾曲部）１７０－ｅを管２０の内部に挿入した状態とする。
その後、制御装置２００は、連続体ロボット１００を後進させて最遠位湾曲部（先頭湾曲部）１７０－ｅを管２０から抜き出す際に、連続体ロボット１００の後進に係る後尾追従制御を行う。この後尾追従制御では、制御装置２００は、基台部１４０の位置が位置ｃ（第３の位置）と位置ｅ（第１の位置）との間で、最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｄ（第２の湾曲角度）と湾曲角度ｅ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した第４の状態となる制御を行う。より詳細に、図８（ｂ）に示す例では、制御装置２００は、基台部１４０の位置が位置ａ（第２の位置）と位置ｅ（第１の位置）との間で、最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｂ（第２の湾曲角度）と湾曲角度ｅ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した第４の状態となる制御を行っている。

３．２）補間法２
先頭追従制御では、後続湾曲部１７０－（ｅ－１）の湾曲角度を実時間で生成する必要があるため、第２章で説明した直角三角形を用いる方法は有効であった。しかしながら、最遠位湾曲部（先頭湾曲部）１７０－ｅでは、少なくとも入力装置３３０から基台部１４０の後進に係る情報が入力される時点において目標湾曲角度を補間することも考えられる。そこで、第２節では、後尾追従制御の補間法２として複数の例を説明する。

図９及び図１０は、本発明の第１の実施形態を示し、図４の制御装置２００に記憶される参照テーブル２１１１として、連続体ロボット１００の後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。具体的に、図９及び図１０は、後尾追従制御の補間法２として複数の例を挙げたものであり、図８に示す要素と同様の要素については同じ符号を付しており、その詳細な説明は省略する。

図９（ａ）は、基台部１４０が位置ｅから前進した位置ａにおいて最遠位湾曲部（先頭湾曲部）１７０－ｅが湾曲角度ｂとされた後、基台部１４０が位置ａから後続湾曲部の長さｌだけ前進した位置ｃにおいて、後尾追従制御の補間法２を行う例を示している。図９（ａ）では、破線で入力装置３１０から入力される最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示し、実線で補間後の最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示している。この図９（ａ）に示す例では、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が、基台部１４０が位置ａにおいて、後続湾曲部１７０－（ｅ－１）の長さｌをもって復帰するように補間する。これは、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度ｄと湾曲角度ａとを結ぶ線分の湾曲角度となり、これは、図８（ａ）及び図８（ｂ）において太実線で示す補間後の後続湾曲部１７０－（ｅ－１）の湾曲角度と一致する。また、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が復帰する基台部１４０の変位ｚ_bは、位置ａに限定されるものではなく、位置ｃよりも原点Ｏの位置に近位の基台部１４０の変位ｚ_bであればよい。

具体的に、図９（ａ）に示す後尾追従制御の補間法２では、制御装置２００は、基台部１４０の位置が位置ｃ（第３の位置）と位置ａ（第２の位置）との間で、最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｄ（第２の湾曲角度）と湾曲角度ａ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した第４の状態となる制御を行っている。

図９（ｂ）は、基台部１４０が位置ｅから前進した位置ａにおいて最遠位湾曲部（先頭湾曲部）１７０－ｅが湾曲角度ｂとされた後、基台部１４０が位置ａから後続湾曲部の長さｌまで前進する途中の位置ｈにおいて、後尾追従制御の補間法２を行う例を示している。この図９（ｂ）に示す例では、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が、基台部１４０が位置ａと位置ｅとの間の位置ｇにおいて、後続湾曲部１７０－（ｅ－１）の長さｌをもって復帰するように補間する。しかしながら、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が復帰する基台部１４０の変位ｚ_bは、位置ｇに限定されるものではなく、位置ｈよりも原点Ｏの位置に近位の基台部１４０の変位ｚ_bであればよい。

図１０（ａ）は、図９（ａ）の変形例であり、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が、後続湾曲部１７０－（ｅ－１）の長さｌと最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌの和をもって直線状に復帰するように補間するものである。

図１０（ｂ）は、図１０（ａ）の変形例であり、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を直線状ではなく曲線状に復帰するように補間するものである。この図１０（ｂ）に示す例では、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を楕円弧状に補間する例を示しているが、本実施形態においてはこれに限定されるものではない。

図１０（ａ）及び図１０（ｂ）に示す例では、制御装置２００は、基台部１４０の位置が位置ｃ（第３の位置）と位置ｅ（第１の位置）との間で、最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｄ（第２の湾曲角度）と湾曲角度ｅ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した第４の状態となる制御を行っている。

なお、本節で説明した後尾追従制御の補間法２では、最遠位湾曲部（先頭湾曲部）１７０－ｅが復帰を開始する基台部１４０の変位ｚ_bを、位置ｃから原点Ｏの位置に近位側として示しているが、位置ｃから遠位側としてもかまわない。

４）シミュレーション
本章では、第３章で説明した後尾追従制御を用いたシミュレーションの結果を説明する。ここで説明するシミュレーションでは、連続体ロボット１００の湾曲部１７０の数ｅを２とし、２つの湾曲部１７０は、図７（ａ）及び図７（ｂ）に示す最遠位湾曲部（先頭湾曲部）１７０－ｅ及び後続湾曲部１７０－（ｅ－１）とする。さらに、ここで説明するシミュレーションでは、最遠位湾曲部（先頭湾曲部）１７０－ｅの長さを０．０２ｍとし、後続湾曲部１７０－（ｅ－１）の長さを０．０２ｍとする。

まず、先頭追従制御の応答について説明する。
先頭追従制御の応答に係るシミュレーションでは、最遠位湾曲部（先頭湾曲部）１７０－ｅは、動作開始後に直進状態で０．０２ｍ前進し、そこで基準軸１０１に対して５０度湾曲し、さらに、０．０２ｍ前進する。

図１１は、本発明の第１の実施形態に係る連続体ロボット１００において、先頭追従制御の応答に係るシミュレーションの結果を示す図である。具体的に、図１１（ａ）～図１１（ｋ）では、実線で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲形状を示し、破線で後続湾曲部１７０－（ｅ－１）の湾曲形状を示している。

次に、後尾追従制御の応答について説明する。

図１２は、本発明の第１の実施形態に係る連続体ロボット１００において、図８（ｂ）に示す後尾追従制御の補間法１の応答に係るシミュレーションの結果を示す図である。具体的に、図１２（ａ）～図１２（ｋ）では、実線で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲形状を示し、破線で後続湾曲部１７０－（ｅ－１）の湾曲形状を示している。図１２では、後続湾曲部１７０－（ｅ－１）の湾曲角度が５０度から０度に復帰した後に、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が５０度から０度に復帰することがわかる。後進時の図１２（ｇ）～図１２（ｋ）において最遠位湾曲部（先頭湾曲部）１７０－ｅが占有する空間は、前進時の図１１（ａ）～図１１（ｅ）において占有する空間に一致していない。しかしながら、後進時の図１２（ａ）～図１２（ｆ）の占有空間は、前進時の図１２（ｆ）～図１２（ｋ）の占有空間と一致する。これにより、連続体ロボット１００において遠位の障害物への接触を回避しながら後退する際に有効な方法であることがわかる。

図１３は、本発明の第１の実施形態に係る連続体ロボット１００において、図９（ａ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。具体的に、図１３（ａ）～図１３（ｋ）では、実線で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲形状を示し、破線で後続湾曲部１７０－（ｅ－１）の湾曲形状を示している。図１３では、後続湾曲部１７０－（ｅ－１）の湾曲角度が５０度から０度に復帰すると同時に、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が５０度から０度に復帰することがわかる。後進時の図１３（ａ）～図１３（ｆ）において最遠位湾曲部（先頭湾曲部）１７０－ｅが占有する空間は、前進時の図１１（ｆ）～図１１（ｋ）において占有する空間に一致していない。しかしながら、後進時の図１３（ｆ）～図１３（ｋ）の占有空間は、前進時の図１１（ａ）～図１１（ｅ）の占有空間と一致する。これにより、連続体ロボット１００において近位の障害物への接触を回避しながら後退する際に有効な方法であることがわかる。

図１４は、本発明の第１の実施形態に係る連続体ロボット１００において、図９（ｂ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。具体的に、図１４（ａ）～図１４（ｋ）では、実線で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲形状を示し、破線で後続湾曲部１７０－（ｅ－１）の湾曲形状を示している。図１４では、後続湾曲部１７０－（ｅ－１）の湾曲角度が５０度から０度に復帰を完了する前の図１４（ｅ）において、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が５０度から復帰を開始し、図１４（ｉ）において０度に復帰が終了することがわかる。後進時の図１４（ｅ）～図１４（ｊ）において最遠位湾曲部（先頭湾曲部）１７０－ｅが占有する空間は、前進時の図１１（ｃ）～図１１（ｇ）において占有する空間に一致していない。しかしながら、後進時の図１４（ａ）～図１４（ｄ）、図１４（ｊ）及び図１４（ｋ）の占有空間は、前進時の図１１（ａ）、図１１（ｂ）及び図１１（ｈ）～図１１（ｅ）の占有空間と一致する。これにより、連続体ロボット１００において最遠位及び最近位の障害物への接触を回避しながら後退する際に有効な方法であることがわかる。

図１５は、本発明の第１の実施形態に係る連続体ロボット１００において、図１０（ａ）に示す後尾追従制御の補間法２の応答に係るシミュレーションの結果を示す図である。具体的に、図１５（ａ）～図１５（ｋ）では、実線で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲形状を示し、破線で後続湾曲部１７０－（ｅ－１）の湾曲形状を示している。図１５では、後進の開始、終了と同時に、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度の復帰が開始、終了される。後進時の全域にわたって、最遠位湾曲部（先頭湾曲部）１７０－ｅが占有する空間は、前進時の占有する空間に一致しない。しかしながら、後進時と前進時の占有する空間の差異は、全域にわたって平均的となる。これにより、連続体ロボット１００において障害物への接触を平均的に回避しながら後退する際に有効な方法であることがわかる。

以上説明した第１の実施形態では、制御装置２００は、連続体ロボット１００を前進させて最遠位湾曲部（先頭湾曲部）１７０－ｅを管２０の内部に挿入した後に、連続体ロボット１００を後進させて最遠位湾曲部（先頭湾曲部）１７０－ｅを管２０から抜き出す際に、基台部１４０の位置が位置ｃ（第３の位置）と位置ｅ（第１の位置）との間で、最遠位湾曲部（先頭湾曲部）１７０－ｅが基準軸１０１に対して湾曲角度ｄ（第２の湾曲角度）と湾曲角度ｅ（第１の湾曲角度）との間の第３の湾曲角度に湾曲した状態となる制御を行うようにしている。
かかる構成によれば、連続体ロボット１００が前進した後に後進する際に、連続体ロボット１００の姿勢が適切に制御され、連続体ロボット１００の破損を防止することが可能となる。

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

第２の実施形態に係る連続体ロボット制御システムの概略構成は、図４に示す第１の実施形態に係る連続体ロボット制御システム１０－１の概略構成と同様である。また、第２の実施形態に係る連続体ロボット１００の概略構成は、図１、図２及び図５に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。

上述した第１の実施形態では、図８～図１０に示すように、基台部１４０の変位ｚ_bにおいて位置ａ（第２の位置）は、基台部１４０における原点Ｏの位置（原点位置）との距離が、最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌよりも大きい場合であった。これに対して、第２の実施形態は、基台部１４０の変位ｚ_bにおいて位置ａ（第２の位置）は、基台部１４０における原点Ｏの位置（原点位置）との距離が、最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌよりも小さい場合を想定した形態である。

図１６は、本発明の第２の実施形態を示し、図４の制御装置２００に記憶される参照テーブル２１１１として、連続体ロボット１００の後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。この図１６において、図８～図１０に示す要素と同様の要素については同じ符号を付しており、その詳細な説明は省略する。

図１６では、基台部１４０の変位ｚ_bにおいて、原点Ｏの位置（原点位置）と位置ａ（第２の位置）との距離が、最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌに満たない場合を示している。図１６では、基台部１４０の変位ｚ_bが位置ａにおいて、入力装置３１０から最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を変更する入力がされた後、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を補間する例を示している。図１６では、破線で入力装置３１０から入力される最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示し、実線で補間後の最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示している。

基台部１４０の変位ｚ_bにおいて、原点Ｏの位置と位置ａとの距離が最遠位湾曲部（先頭湾曲部）１７０－ｅの長さｌに満たないため、第１の実施形態で説明した後尾追従制御の補間法１を用いると、原点Ｏの位置で最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が零に収束しない。そこで、第２の実施形態では、第１の実施形態の補間法２を用いることが好適であるが、この際、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度が復帰する基台部１４０の変位ｚ_bは、原点Ｏの位置と位置ａとの間の位置が好ましい。

具体的に、図１６に示す例では、制御装置２００は、連続体ロボット１００の後尾追従制御を行う際に、基台部１４０が原点Ｏの位置（原点位置）において最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を零にする制御を行う。

第２の実施形態においても、上述した第１の実施形態と同様に、連続体ロボット１００が前進した後に後進する際に、連続体ロボット１００の姿勢が適切に制御され、連続体ロボット１００の破損を防止することが可能となる。

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

第３の実施形態に係る連続体ロボット制御システムの概略構成は、図４に示す第１の実施形態に係る連続体ロボット制御システム１０－１の概略構成と同様である。また、第３の実施形態に係る連続体ロボット１００の概略構成は、図１、図２及び図５に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。

上述した第１の実施形態及び第２の実施形態では、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を、基台部１４０の変位ｚ_bに基づいて復帰させる形態であった。しかしながら、例えば、上述した第１の実施形態の図９（ｂ）において、基台部１４０の変位ｚ_bにおける位置ｈと位置ｇとが近接しているときには、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度における復帰動作が急峻になる。これは、上述した第２の実施形態において、位置ａが原点Ｏの位置（原点位置）に近接しているときにも生ずる。

そこで、第３の実施形態では、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を、基台部１４０の変位ｚ_bに基づかずに復帰させる制御方法を示す。

図１７は、本発明の第３の実施形態を示し、図４の制御装置２００に記憶される参照テーブル２１１１として、連続体ロボット１００の後尾追従制御（先頭追従制御も含みうる）の参照テーブルの一例を示す図である。この図１７において、図８～図１０に示す要素と同様の要素については同じ符号を付しており、その詳細な説明は省略する。

具体的に、図１７に示す参照テーブル２１１１は、横軸に時間をとり、縦軸に最遠位湾曲部（先頭湾曲部）の湾曲角度θをとったグラフとして示されている。図１７では、破線で入力装置３１０から入力される最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示し、実線で補間後の最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を示している。

まず、図１７では、時刻Ａにおいて、入力装置３１０から最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度を変更する入力がされ、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度がＡからＢとなる。その後、連続体ロボット１００において基台部１４０の変位が微小の前進動作がなされた状態（第３の状態）又は当該前進動作がされなかった状態（第２の状態）で、短時間で連続体ロボット１００の後進動作が指令されたときは、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度における変化率に速度制限を設けて、湾曲角度Ｃから湾曲角度Ｄのように復帰させる。即ち、本実施形態では、制御装置２００は、第３の状態又は第２の状態から後進動作に係る第４の状態への遷移が所定時間よりも短時間に行われるように指令を受けた場合には、最遠位湾曲部（先頭湾曲部）１７０－ｅの湾曲角度の時間における変化率を制限する。

第３の実施形態においても、上述した第１の実施形態と同様に、連続体ロボット１００が前進した後に後進する際に、連続体ロボット１００の姿勢が適切に制御され、連続体ロボット１００の破損を防止することが可能となる。

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

第４の実施形態に係る連続体ロボット１００の概略構成は、図１、図２及び図５に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。

図１８は、本発明の第４の実施形態に係る連続体ロボット制御システム１０－４の概略構成の一例を示す図である。この図１８において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。この図１８に示す連続体ロボット制御システム１０－４は、図１及び図２に示す連続体ロボット１００の制御を行うシステムである。また、連続体ロボット制御システム１０－４は、図１８に示すように、連続体ロボット１００、制御装置２００、並びに、各種の入力装置３２０、３３０及び３４０を有して構成されている。

入力装置３２０は、図１に示す入力装置３１０が入力する最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_Lに加えて、最遠位湾曲部（先頭湾曲部）の目標旋回角度ζ_Lを制御装置２００に入力する装置である。

制御装置２００は、図１８に示すように、角度算出部２１０、切替部２２０、及び、運動学演算部（Kinematics）２３０を有して構成されている。

角度算出部２１０には、入力装置３２０から最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_L及び目標旋回角度ζ_L、入力装置３３０から基台部１４０の変位ｚ_b、入力装置３４０から複数の湾曲部１７０の長さｌの情報が入力される。そして、角度算出部２１０は、入力された情報に基づいて、後続湾曲部の目標湾曲角度θ_F及び目標旋回角度ζ_Fの算出や、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_Lの変更に伴う変更後の後続湾曲部の目標湾曲角度θ_F'及び目標旋回角度ζ_F'を算出する。さらに、角度算出部２１０は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度θ_L及び目標旋回角度ζ_Lや後続湾曲部の目標湾曲角度θ_F及び目標旋回角度ζ_Fを記憶し、連続体ロボット１００の基台部１４０が後進するときの最遠位湾曲部の目標湾曲角度θ_L'及び目標旋回角度ζ_L'を算出する。

切替部２２０は、最遠位湾曲部（先頭湾曲部）の目標湾曲角度及び目標旋回角度として、入力装置３２０から入力された目標湾曲角度θ_L及び目標旋回角度ζ_Lと、角度算出部２１０により算出された目標湾曲角度θ_L'及び目標旋回角度ζ_L'とを、基台部１４０の変位ｚ_bの進行方向に応じて切り替える。具体的に、本実施形態では、切替部２２０は、基台部１４０の変位ｚ_bの進行方向が前進の方向である場合には、入力装置３２０から入力された目標湾曲角度θ_L及び目標旋回角度ζ_Lを選択する切り替えを行う。また、切替部２２０は、基台部１４０の変位ｚ_bの進行方向が後進の方向である場合には、角度算出部２１０により算出された目標湾曲角度θ_L'及び目標旋回角度ζ_L'を選択する切り替えを行う。

運動学演算部２３０には、切替部２２０から出力された最遠位湾曲部（先頭湾曲部）の目標湾曲角度（θ_Lまたはθ_L'）及び目標旋回角度（ζ_Lまたはζ_L'）と、角度算出部２１０で算出された後続湾曲部の目標湾曲角度θ_F'及び目標旋回角度ζ_F'の情報が入力される。そして、運動学演算部２３０は、入力された情報に基づいて、駆動部である各アクチュエータで最遠位湾曲部（先頭湾曲部）及び後続湾曲部のワイヤを駆動させる際の駆動変位を演算する。図１８では、運動学演算部２３０の演算により得られた最遠位湾曲部（先頭湾曲部）のワイヤの駆動変位と後続湾曲部のワイヤの駆動変位をまとめて、ワイヤの駆動変位ｌ_pとして図示している。そして、本実施形態に係る連続体ロボット１００は、それぞれのワイヤの駆動変位ｌ_pに応じて、最遠位湾曲部（先頭湾曲部）及び後続湾曲部の湾曲制御が行われることになる。

上述した第１～第３の実施形態では、ｘｚ平面内での連続体ロボット１００の後尾追従制御の制御方法について主として説明した。本実施形態では、３次元空間での後尾追従制御を行う形態である。

連続体ロボット１００の湾曲部１７０の湾曲角度及び旋回角度を制御するためのアクチュエータの駆動変位を求めるため、運動学の導出を行う。記号の定義を以下に示す。
ｌ_d：湾曲部の中心軸の長さ
θ_n：第ｎの湾曲部の（遠位端の）湾曲角度
ζ_n：第ｎの湾曲部の（遠位端の）旋回角度
ρ_n：第ｎの湾曲部の曲率半径
ζ_refn：第ｎの湾曲部の（遠位端の）目標旋回角度

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

まず、第１の湾曲部１７０－１のａワイヤ、ｂワイヤ及びｃワイヤの駆動変位ｌ_p1a、ｌ_p1b及びｌ_p1cと、第１の湾曲部１７０－１の湾曲角度θ₁及び旋回角度ζ₁との関係は、以下の（１０）式となる。

次に、複数の湾曲部を有する連続体ロボット１００において、第ｎの湾曲部１７０－ｎのａワイヤ、ｂワイヤ及びｃワイヤの駆動変位ｌ_pna、ｌ_pnb及びｌ_pncと、第ｎの湾曲部１７０－ｎの湾曲角度θ_n及び旋回角度ζ_nとの関係を求める。また、連続体ロボット１００の湾曲部の数をｅとし、第ｎの湾曲部を駆動するワイヤの位相角ξ_nを以下の（１１）式とする。

これにより、第ｎの湾曲部１７０－ｎのワイヤ駆動変位ｌ_pna、ｌ_pnb及びｌ_pncは、以下の（１２）式となる。

入力装置３２０から、最遠位湾曲部（先頭湾曲部）１７０－ｅに対して目標湾曲角度θ_L及び目標旋回角度ζ_Lが入力されたとき、制御装置２００は、（１２）式を用いてワイヤの駆動変位を求めることで、最遠位湾曲部（先頭湾曲部）１７０－ｅの角度を制御することができる。そして、本実施形態における後尾追従制御は、上述したｘｚ平面における第１～第３の実施形態と同様に、目標湾曲角度を補間して記憶部２１１から読み出せばよく、また、上述した第１～第３の実施形態の湾曲角度を旋回角度と読み替えて同様のアルゴリズムによって目標旋回角度を算出すればよい。そして、本実施形態では、複数の湾曲部１７０の各々に対して、（１２）式を用いてワイヤの駆動変位を求めることで、３次元空間内での連続体ロボット１００の後尾追従制御を行うことができる。

第４の実施形態においても、上述した第１の実施形態と同様に、連続体ロボット１００が前進した後に後進する際に、連続体ロボット１００の姿勢が適切に制御され、連続体ロボット１００の破損を防止することが可能となる。

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

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

１０：連続体ロボット制御システム、２０：管、１００：連続体ロボット、１４０：基台部、１７０：湾曲部、２００：制御装置、２１０：角度算出部、２１１：記憶部、２１１１：参照テーブル、２１２：参照テーブル書換部、２１３：情報入力部、２２０：切替部、２３０：運動学演算部（Kinematics）、３１０～３４０：入力装置

Claims (9)

1. 基台部と、
   遠位用の線状部材が駆動されることによって湾曲する遠位湾曲部と、
   前記遠位用の線状部材を駆動する駆動部と、
   前記遠位湾曲部と前記基台部との間に設けられた連結部と、
   を有する連続体ロボットの制御を行う連続体ロボット制御システムであって、
   前記連続体ロボットを前進および後進させる移動手段と、
   前記連続体ロボットの前記基台部が第１の位置にあって前記遠位湾曲部が基準軸に対して第１の湾曲角度に湾曲した第１の状態、前記基台部が前記第１の位置よりも前記前進した第２の位置にあって前記遠位湾曲部が前記基準軸に対して前記第１の湾曲角度とは異なる第２の湾曲角度に湾曲した第２の状態、および、前記基台部が前記第２の位置よりも前記前進した第３の位置にある第３の状態の順に遷移することで、前記遠位湾曲部を管の内部に挿入した後に、前記連続体ロボットを前記後進させて前記遠位湾曲部を前記管から抜き出す際に、前記基台部の位置が前記第３の位置と前記第１の位置との間で、前記遠位湾曲部が前記基準軸に対して前記第２の湾曲角度と前記第１の湾曲角度との間の第３の湾曲角度に湾曲した第４の状態をとるように制御する制御手段と、
   を有することを特徴とする連続体ロボット制御システム。
2. 前記制御手段は、前記基台部の位置が前記第２の位置と前記第１の位置との間で、前記第４の状態をとるように制御することを特徴とする請求項１に記載の連続体ロボット制御システム。
3. 前記制御手段は、前記基台部の位置が前記第３の位置と前記第２の位置との間で、前記第４の状態をとるように制御することを特徴とする請求項１に記載の連続体ロボット制御システム。
4. 前記制御手段は、前記基台部における原点位置と前記第２の位置との距離が、前記遠位湾曲部の長さに満たない場合には、前記第４の状態をとるように制御する際に前記原点位置において前記第３の湾曲角度を零にすることを特徴とする請求項１乃至３のいずれか１項に記載の連続体ロボット制御システム。
5. 前記制御手段は、前記第３の状態または前記第２の状態から前記第４の状態への遷移が所定時間よりも短時間に行われるように指令を受けた場合には、前記第３の湾曲角度の時間における変化率を制限することを特徴とする請求項１乃至４のいずれか１項に記載の連続体ロボット制御システム。
6. 前記制御手段は、前記遠位湾曲部の前記第１の湾曲角度、前記第２の湾曲角度および前記第３の湾曲角度における湾曲角度に加えて、前記遠位湾曲部の旋回角度の制御を行うことを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボット制御システム。
7. 前記連結部は、前記遠位湾曲部に後続する湾曲部であって、後続用の線状部材が駆動されることによって湾曲する後続湾曲部を含み、
   前記駆動部は、前記遠位用の線状部材とは独立して前記後続用の線状部材を駆動することを特徴とする請求項１乃至６のいずれか１項に記載の連続体ロボット制御システム。
8. 基台部と、
   遠位用の線状部材が駆動されることによって湾曲する遠位湾曲部と、
   前記遠位用の線状部材を駆動する駆動部と、
   前記遠位湾曲部と前記基台部との間に設けられた連結部と、
   を有する連続体ロボットの制御を行う連続体ロボット制御方法であって、
   前記連続体ロボットを前進および後進させる移動ステップと、
   前記連続体ロボットの前記基台部が第１の位置にあって前記遠位湾曲部が基準軸に対して第１の湾曲角度に湾曲した第１の状態、前記基台部が前記第１の位置よりも前記前進した第２の位置にあって前記遠位湾曲部が前記基準軸に対して前記第１の湾曲角度とは異なる第２の湾曲角度に湾曲した第２の状態、および、前記基台部が前記第２の位置よりも前記前進した第３の位置にある第３の状態の順に遷移することで、前記遠位湾曲部を管の内部に挿入した後に、前記連続体ロボットを前記後進させて前記遠位湾曲部を前記管から抜き出す際に、前記基台部の位置が前記第３の位置と前記第１の位置との間で、前記遠位湾曲部が前記基準軸に対して前記第２の湾曲角度と前記第１の湾曲角度との間の第３の湾曲角度に湾曲した第４の状態をとるように制御する制御ステップと、
   を有することを特徴とする連続体ロボット制御方法。
9. コンピュータを、請求項１乃至７のいずれか１項に記載の連続体ロボット制御システムの各手段として機能させるためのプログラム。

Images