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

Abstract

【課題】連続体ロボットの操作を安全に行える仕組みを提供する。【解決手段】ブロックＦＴＬは、最遠位湾曲区間の目標湾曲角度θlt及び目標旋回角度ζltと、基台部の変位とに基づいて、後続湾曲区間の目標湾曲角度θfFTL及び目標旋回角度ζfFTLを算出し、切替スイッチ部３３０は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θfFTL及び目標旋回角度ζfFTLと、後続操作手段に相当するブロックＰlから得られる後続湾曲区間の目標湾曲角度θlf及び目標旋回角度ζlfとのうちのいずれか一方を選択するための切替処理を行い、運動学演算部３４０は、切替スイッチ部３３０で選択された後続湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボットの駆動部で後続湾曲区間のワイヤを駆動させる際の駆動変位を演算する。【選択図】図４

Description

本発明は、ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部を備える連続体ロボットの制御システム及びその制御方法、並びに、当該制御システムとしてコンピュータを機能させるためのプログラムに関するものである。

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

特許文献１には、湾曲可能部の先端にカメラを備える連続体ロボットに対して、カメラの視線の方向を一定に保ったまま先端の位置を移動させる動作や、カメラの視線が常に遠方の１点を通るように先端の位置と角度を移動させるといった特徴的な動作（以下、「観察動作制御」と呼ぶ）を、操縦者が簡単な操作を行うだけで実現できる制御方法が記載されている。

国際公開第２０１７／００３４６８号

具体的に、特許文献１では、湾曲可能部の初期の湾曲姿勢から観察動作制御に移行するため、湾曲可能部の全ての湾曲区間に対する追加の湾曲量を操縦者による操作部の操作量から代数的に求めている。そのため、特許文献１に記載の制御では、湾曲可能部の初期の湾曲姿勢によっては、操縦者による操作部の操作量が軽微であっても、各湾曲区間に対する追加の湾曲量が大きく制御されることがありうる。連続体ロボットは、一般的に、先頭の最遠位湾曲区間の湾曲姿勢を後続湾曲区間に湾曲区間の長さをもって連続的に伝播させる制御（以下、「先頭追従制御」と呼ぶ）を用いることによって、狭小な空間の対象物における狭い経路に沿って動作させることができる。そして、このような狭小な空間において、特許文献１に記載の制御が行われれば、意図せずに連続体ロボットが対象物と接触してしまい、その結果、対象物が傷ついたり連続体ロボット自体が破損したりして、連続体ロボットの操作を安全に行えない問題が生じうる。

本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの操作を安全に行える仕組みを提供することを目的とする。

本発明の連続体ロボットの制御システムは、ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出手段と、操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作手段と、前記後続算出手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替手段と、前記後続切替手段で選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算手段と、を有する。
また、本発明は、上述した連続体ロボットの制御システムによる連続体ロボットの制御方法、及び、上述した連続体ロボットの制御システムとしてコンピュータを機能させるためのプログラムを含む。

本発明によれば、連続体ロボットの操作を安全に行うことができる。

本発明の第１の実施形態に係る連続体ロボットの概略構成の一例を示す図である。 図１に示す連続体ロボットの概略構成のうち、１つの湾曲区間に関する詳細な概略構成の一例を示す図である。 本発明の第１の実施形態を示し、図１に示す連続体ロボットの３つの湾曲区間を操作するための操作装置の概略構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 図２に示す３つのワイヤに相当する第ｎの湾曲区間の３つのワイヤ（ａワイヤ～ｃワイヤ）のｘｙ平面における配置例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの先頭追従制御の一例を示す図である。 本発明の第１の実施形態を示し、図４のブロックＦＴＬによる後続湾曲区間の目標湾曲角度及び目標旋回角度の算出処理の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第３例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第１例を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の第２例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムによる連続体ロボットの制御方法におけるシミュレーション結果の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。 本発明の第５の実施形態を示し、図１に示す連続体ロボットの３つの湾曲区間を操作するための操作装置の概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボットの制御システムの概略構成の一例を示す図である。

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

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

図１は、本発明の第１の実施形態に係る連続体ロボット１００の概略構成の一例を示す図である。図１には、連続体ロボット１００の一構成として、基台部１４０及び湾曲可能部１７０が図示されている。

湾曲可能部１７０は、ワイヤが駆動することによって湾曲する複数の湾曲区間１７１～１７３が設けられた構成部（湾曲区間が複数設けられた構成部）である。具体的に、図１には、（ｎ－２）に係る湾曲区間１７１、（ｎ－１）に係る湾曲区間１７２、ｎに係る湾曲区間１７３のｎ個（３個）の湾曲区間が図示されている。また、図１では、湾曲区間１７３の湾曲角度θ_n及び旋回角度ζ_nを示すとともに、湾曲区間１７２の湾曲角度θ_n-1及び旋回角度ζ_n-1を示し、湾曲区間１７１の湾曲角度θ_n-2及び旋回角度ζ_n-2を示している。基台部１４０は、湾曲可能部１７０を支える構成部であり、その内部には、湾曲区間１７１～１７３のそれぞれのワイヤを駆動させるための駆動部であるアクチュエータ（図１では不図示）が設けられている。また、図１では、基台部１４０の上面１４１の所定位置（例えば中心の位置）に原点Ｏをとり、連続体ロボット１００の進行方向（例えば前進する方向）をｚ方向とし、このｚ方向と直交する方向であって相互に直交する方向をｘ方向及びｙ方向とした、ｘｙｚ座標系を図示している。このように、連続体ロボット１００は、湾曲可能部１７０を構成する複数の湾曲区間１７１～１７３による湾曲動作に加えて、ｚ方向に移動が可能となっている。この際、図１では、連続体ロボット１００のｚ方向の移動量（前進量）を示す指標として基台部１４０の変位ｚ_bを図示している。

図１において、湾曲区間１７３は、湾曲可能部１７０を構成する複数の湾曲区間１７１～１７３のうち、基台部１４０から最も遠い位置にある最遠位湾曲区間である。湾曲区間１７２（湾曲区間１７１も同様）は、最遠位湾曲区間である湾曲区間１７３と基台部１４０との間に位置し、連続体ロボット１００が前進する際に最遠位湾曲区間である湾曲区間１７３の後に続く後続湾曲区間である。なお、連続体ロボット１００が前進する際に、最遠位湾曲区間である湾曲区間１７３は、先頭の湾曲区間となる。

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

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

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

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

なお、図２に示す例では、湾曲区間１７１に関する詳細な概略構成のみを図示してその説明を行った。この点、図１に示す湾曲区間１７２及び湾曲区間１７３についても、図２に示す湾曲区間１７１に関する詳細な概略構成と同様に、それぞれが、ワイヤ１１１～１１３に相当するワイヤや、アクチュエータ１３１～１３３に相当するアクチュエータ、遠位端１６０に相当する遠位端、及び、ワイヤガイド１６１～１６４に相当するワイヤガイドを有して構成されている。ここで、一般化して、第ｎの湾曲区間を駆動するａ～ｃワイヤの駆動変位をｌ_pna，ｌ_pnb，ｌ_pncとする。

図３は、本発明の第１の実施形態を示し、図１に示す連続体ロボット１００の３つの湾曲区間１７１～１７３を操作するための操作装置２００の概略構成の一例を示す図である。以下の説明では、図３に示す第１の実施形態に係る操作装置２００を「操作装置２００－１」として説明を行う。

操作装置２００－１は、図３に示すように、第１の湾曲区間１７１を操作するための第１の操作部２１０、第２の湾曲区間１７２を操作するための第２の操作部２２０、及び、第３の湾曲区間１７３を操作するための第３の操作部２３０を有して構成されている。

第３の操作部２３０において、構成部２３１～２３３は、最遠位湾曲区間である第３の湾曲区間１７３の湾曲角度θ_nに関する構成部である。具体的に、操作レバー２３１は、第３の湾曲区間１７３の湾曲角度θ_nに対する操作レバーであり、モータ２３２により駆動される。また、その回転角度は、角度センサ２３３により検出される。この際、モータ２３２及び角度センサ２３３は、操作レバー２３１の筐体（不図示）に固定されている。また、第３の操作部２３０において、構成部２３４～２３６は、最遠位湾曲区間である第３の湾曲区間１７３の旋回角度ζ_nに関する構成部である。具体的に、操作レバー２３４は、第３の湾曲区間１７３の旋回角度ζ_nに対する操作レバーであり、モータ２３５により駆動される。また、その回転角度は、角度センサ２３６により検出される。この際、モータ２３５及び角度センサ２３６は、操作レバー２３４の筐体（不図示）に固定されている。このように、第３の操作部２３０は、例えば操縦者に操作されることによって、最遠位湾曲区間である第３の湾曲区間１７３の目標湾曲角度θ_n及び目標旋回角度ζ_nを入力するための最遠位操作手段に相当する構成である。

第２の操作部２２０において、構成部２２１～２２３は、後続湾曲区間である第２の湾曲区間１７２の湾曲角度θ_n-1に関する構成部である。具体的に、操作レバー２２１は、第２の湾曲区間１７２の湾曲角度θ_n-1に対する操作レバーであり、モータ２２２により駆動される。また、その回転角度は、角度センサ２２３により検出される。この際、モータ２２２及び角度センサ２２３は、操作レバー２２１の筐体（不図示）に固定されている。また、第２の操作部２２０において、構成部２２４～２２６は、後続湾曲区間である第２の湾曲区間１７２の旋回角度ζ_n-1に関する構成部である。具体的に、操作レバー２２４は、第２の湾曲区間１７２の旋回角度ζ_n-1に対する操作レバーであり、モータ２２５により駆動される。また、その回転角度は、角度センサ２２６により検出される。この際、モータ２２５及び角度センサ２２６は、操作レバー２２４の筐体（不図示）に固定されている。このように、第２の操作部２２０は、例えば操縦者に操作されることによって、後続湾曲区間である第２の湾曲区間１７２（第２の後続湾曲区間）の目標湾曲角度θ_n-1及び目標旋回角度ζ_n-1を入力するための第２の後続操作手段に相当する構成である。

第１の操作部２１０において、構成部２１１～２１３は、後続湾曲区間である第１の湾曲区間１７１の湾曲角度θ_n-2に関する構成部である。具体的に、操作レバー２１１は、第１の湾曲区間１７１の湾曲角度θ_n-2に対する操作レバーであり、モータ２１２により駆動される。また、その回転角度は、角度センサ２１３により検出される。この際、モータ２１２及び角度センサ２１３は、操作レバー２１１の筐体（不図示）に固定されている。また、第１の操作部２１０において、構成部２１４～２１６は、後続湾曲区間である第１の湾曲区間１７１の旋回角度ζ_n-2に関する構成部である。具体的に、操作レバー２１４は、第１の湾曲区間１７１の旋回角度ζ_n-2に対する操作レバーであり、モータ２１５により駆動される。また、その回転角度は、角度センサ２１６により検出される。この際、モータ２１５及び角度センサ２１６は、操作レバー２１４の筐体（不図示）に固定されている。このように、第１の操作部２１０は、例えば操縦者に操作されることによって、後続湾曲区間である第１の湾曲区間１７１（第１の後続湾曲区間）の目標湾曲角度θ_n-2及び目標旋回角度ζ_n-2を入力するための第１の後続操作手段に相当する構成である。

以上説明したように、操作装置２００－１は、各湾曲区間１７１～１７３に対して、湾曲角度θ及び旋回角度ζのそれぞれについて操作レバー２１１～２３１及び２１４～２３４を備えている。また、本実施形態では、図４で後述する連続体ロボットの制御システム３００が、連続体ロボット１００の湾曲角度θもしくは旋回角度ζを任意の目標角度に制御しているとき、操作レバー２１１～２３１及び２１４～２３４をその角度と同様になるように制御する。

さらに、操作レバー２１１～２３１及び２１４～２３４には、操縦者の接触を検出するセンサ（不図示）が設けられている。この操作レバー２１１～２３１及び２１４～２３４に設けるセンサとしては、押しボタンスイッチが装備されていたり静電容量を検出するセンサが装備されていたりしてもよい。さらに、基台部１４０の変位ｚ_bを操作するための操作ボタン（不図示）を設けてもよく、また、動作モードを変更するためのスイッチ（不図示）を複数設けてもよい。

図４は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－１」として説明を行う。具体的に、図４には、先頭追従制御と観察動作制御との切り替え制御系のブロック線図が示されている。

図４に示す連続体ロボットの制御システム３００－１において、ブロックＰ_sは、制御対象である連続体ロボット１００（より具体的には、図２に示す駆動部であるアクチュエータ１３１～１３３に相当するアクチュエータ）を示している。そして、図４において、ブロックＰ_sからの出力であるｌ_pは、ワイヤの駆動変位を示すベクトルである。また、図４に示す連続体ロボットの制御システム３００－１において、ブロックＫ_sは、ワイヤの目標駆動変位ｌ_prefに整定させるための位置制御系である。

また、図４に示す連続体ロボットの制御システム３００－１において、ブロックＰ_lは、モータを有する後続湾曲区間を操作するための操作系（後続操作手段）である。このブロックＰ_lは、例えば、図３に示す例では、後続湾曲区間である第２の湾曲区間１７２及び第１の湾曲区間１７１を操作するための第２の操作部２２０及び第１の操作部２１０に相当する操作系である。また、図４において、ｄ_θlf及びｄ_ζlfは、それぞれ、操縦者が後続湾曲区間の湾曲角度の操作レバー及び旋回角度の操作レバーに印加する操作トルクを表している。そして、図４において、ブロックＰ_lの出力である後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfは、それぞれ、後続湾曲区間の湾曲角度及び旋回角度を操作するための第２の操作部２２０及び第１の操作部２１０における操作レバーによる角度である。また、図４において、ブロックＫ_lは、それを目標値に整定させるための位置制御系であり、また、このブロックＫ_lの入力端及び出力端にそれぞれ接続されるブロックＧ_in及びＧ_outは、０から１の値をとるゲインである。

図４に示す連続体ロボットの制御システム３００－１において、入力装置３１０は、最遠位湾曲区間（図１に示す例では、第３の湾曲区間１７３）の目標湾曲角度θ_lt及び目標旋回角度ζ_ltを、ブロックＦＴＬ及び運動学演算部３４０に入力する装置である。そして、入力装置３１０の出力である最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltは、それぞれ、最遠位湾曲区間の湾曲角度及び旋回角度を操作するための第３の操作部２３０における操作レバー２３１及び２３４による角度である。なお、本実施形態では、最遠位湾曲区間である第３の湾曲区間１７３を操作するための図３に示す第３の操作部２３０に含まれる操作レバー２３１及び２３４は、モータ駆動されない。

図４に示す連続体ロボットの制御システム３００－１において、入力装置３２０は、連続体ロボット１００の進行方向（例えば前進する方向）における基台部１４０の変位ｚ_bを、ブロックＦＴＬに入力する装置である。

図４に示す連続体ロボットの制御システム３００－１において、ブロックＦＴＬは、先頭追従制御アルゴリズムの構成例である。このブロックＦＴＬは、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltと、入力装置３２０から入力された基台部１４０の変位ｚ_bと、に基づいて、後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLを算出する後続算出手段である。より具体的に、目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLは、後続湾曲区間に対する先頭追従制御系による目標値ベクトルを表している。

図４に示す連続体ロボットの制御システム３００－１において、切替スイッチ部３３０は、第１のスイッチ３３１及び第２のスイッチ３３２を備えて構成されている。第１のスイッチ３３１は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLと、ブロックＰ_lから得られる後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfと、のうちのいずれか一方を選択するための切替処理を行うスイッチ（後続切替手段）である。第２のスイッチ３３２は、０または１の信号が入力されるスイッチであり、入力信号に応じて切替を行う。この切替スイッチ部３３０は、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、図４に示している２極双投形スイッチの状態となっている。即ち、ここで説明した先頭追従制御が行われているときには、第１のスイッチ３３１は、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLを選択するための切替処理を行う。そして、切替スイッチ部３３０は、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、本実施形態の制御系は、図４に示している２極双投形スイッチの状態とは逆の方向に切り替える。即ち、ここで説明した後続湾曲区間の操作レバーへの操縦者の接触が検知されると、第１のスイッチ３３１は、ブロックＰ_lから得られる後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfを選択するための切替処理を行う。これにより、操縦者は、連続体ロボット１００を先頭追従制御による姿勢（湾曲形状）から継ぎ目なく追加の操作を加えることが可能となる。

図４に示す連続体ロボットの制御システム３００－１において、運動学演算部（Ｋｉｎｅｍａｔｉｃｓ）３４０は、第１のスイッチ３３１で選択された後続湾曲区間の目標湾曲角度θ及び目標旋回角度ζに基づいて、連続体ロボット１００の駆動部で後続湾曲区間のワイヤを駆動させる際の目標駆動変位ｌ_prefを演算する演算手段である。さらに、本実施形態では、運動学演算部３４０は、入力装置３１０から入力された最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltに基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の目標駆動変位ｌ_prefも演算する。

１）モデリング
本章では、連続体ロボット１００の湾曲角度θ及び旋回角度ζを制御するためのアクチュエータによるワイヤの目標駆動変位を求めるため、運動学の導出を行う。
本章で用いる記号の定義を以下に示す。
ｌ_d：湾曲区間の中心軸の長さ
θ_n：遠位端の湾曲角度
ζ_n：遠位端の旋回角度
ρ_n：湾曲区間の曲率半径

上述したように、本実施形態では、図２に示すワイヤ１１１～１１３に相当する各湾曲区間の３つのワイヤを、ｘｙ面内において反時計回りにａワイヤ、ｂワイヤ及びｃワイヤと呼ぶ。また、第ｎの湾曲区間を駆動するａ～ｃワイヤの駆動変位をｌ_pna，ｌ_pnb，ｌ_pncとする。

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

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

まず、第１の湾曲区間（図１及び図２の湾曲区間１７１に相当）におけるａ～ｃワイヤの駆動変位ｌ_p1a，ｌ_p1b，ｌ_p1cと、第１の湾曲区間の遠位端の湾曲角度θ₁及び旋回角度ζ₁との関係は、以下の（１）式となる。

次に、複数の湾曲区間を有する連続体ロボット１００の第ｎの湾曲区間におけるａ～ｃワイヤの駆動変位ｌ_pna，ｌ_pnb，ｌ_pncと、第ｎの湾曲区間の遠位端の湾曲角度θ_n及び旋回角度ζ_nとの関係を求める。ここで、湾曲区間数をｅとし、第ｎの湾曲区間を駆動するワイヤの位相角ξ_nを以下の（２）式とする。

これより、第ｎの湾曲区間におけるａ～ｃワイヤの駆動変位ｌ_pna，ｌ_pnb，ｌ_pncは、以下の（３）式となる。

次に、図３に示す操作装置２００－１のモデリングを行う。
第ｎの湾曲区間の湾曲角度θ_nを指令する操作レバーのモータを含む全ての慣性モーメントをＪ_θnとし、第ｎの湾曲区間の旋回角度ζ_nを指令する操作レバーのモータを含む全ての慣性モーメントをＪ_ζnとする。また、第ｎの湾曲区間の湾曲角度θ_nを指令する操作レバーに与える操縦者の操作トルクをｄ_θnとし、第ｎの湾曲区間の旋回角度ζ_nを指令する操作レバーに与える操縦者の操作トルクをｄ_ζnとする。そして、第ｎの湾曲区間の湾曲角度θ_nを指令する操作レバーに与える制御トルクをｕ_θnとし、第ｎの湾曲区間の旋回角度ζ_nを指令する操作レバーに与える制御トルクをｕ_ζnとすると、運動方程式は、以下の（４）式のようになる。

２）制御系設計
本章では、先頭追従制御系と、その途中で観察動作制御を行うための制御系の設計を行う。

図６は、本発明の第１の実施形態に係る連続体ロボット１００の先頭追従制御の一例を示す図である。この図６において、図１に示す構成と同様の構成については同じ符号を付している。また、図６では、図１に示すｚ方向を紙面下側から紙面上側に向かう方向にとっている。また、図６には、基台部１４０及び湾曲可能部１７０を含み構成された連続体ロボット１００が進む目標経路６１０を点線で図示している。

ここで、先頭追従制御とは、図６に示すように、湾曲可能部１７０の最遠位湾曲区間が通る経路（目標経路６１０）と同じ経路を後続湾曲区間が通るように制御する方法である。ここで、以下の説明では、必要に応じて、連続体ロボット１００の先頭である最遠位湾曲区間（図１に示す湾曲区間１７３）を「遠位湾曲区間」として説明し、上述した最遠位湾曲区間に連なる後続湾曲区間を「追従湾曲区間」として説明することがある。この際、先頭追従制御の一例としては、入力された遠位湾曲区間の第１の目標湾曲角度に応じて設定される、連続体ロボット１００の前進移動に対応した追従湾曲区間の湾曲角度に関する第１の湾曲角度のプロファイルに従って、追従湾曲区間の湾曲角度が第１の目標湾曲角度に至るよう構成されている制御がある。

図６において、時点６０１は、基台部１４０の上面からｚ方向に延びる湾曲可能部１７０が湾曲していない初期状態を示している。その後、図６においては、時点６０２，時点６０３，時点６０４，時点６０５と時間が経過するのにしたがって、基台部１４０がｚ方向に移動して湾曲可能部１７０が湾曲する様子が示されている。

このような先頭追従制御により、連続体ロボット１００は、空間をすり抜けるように進むことができる。なお、先頭追従制御は、予め目標経路６１０が定められていることは必須ではなく、例えば、最遠位湾曲区間の湾曲角度を、後続湾曲区間の湾曲角度に当該湾曲区間の長さをもって連続的に伝播させてもよい。この方法を用いると、例えば、操縦者は、ジョイスティックなどで最遠位湾曲区間の湾曲角度と基台部１４０の変位（移動量（前進量））に対して指令を与えることで、連続体ロボット１００を実時間で先頭追従制御することができる。

２.１）先頭追従制御
図７は、本発明の第１の実施形態を示し、図４のブロックＦＴＬによる後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLの算出処理の一例を示す図である。なお、以下の図７の説明では、最遠位湾曲区間として図１の湾曲区間１７３を適用し、後続湾曲区間として図１の湾曲区間１７２を適用した場合を想定した例について説明する。図７において、グラフ上の座標は、基台部１４０の位置と湾曲区間（湾曲セクション）の湾曲角度との対を示している。説明のため、以下では、座標ａに対応する湾曲角度のことを"角度ａ"、そして座標ａに対応する基台部１４０の位置（変位）を位置ａ（変位ａ）と呼ぶことがある。

図７（ａ）において、横軸は、基台部１４０の変位ｚ_bを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の湾曲角度θを示している。図７（ａ）において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標湾曲角度θ_ltに対応する、最遠位湾曲区間の湾曲角度のプロファイルを示している。また、図７（ａ）において、太破線の「Follower」は、例えば後続湾曲区間の目標湾曲角度θ_lfに対応する、追従湾曲区間の湾曲角度のプロファイルを示している。

図７（ｂ）において、横軸は、基台部１４０の変位ｚ_bを示し、また、縦軸は、最遠位湾曲区間及び後続湾曲区間の目標旋回角度ζを示している。図７（ｂ）において、破線の「Leader」は、操縦者等により入力装置３１０を介して入力された最遠位湾曲区間の目標旋回角度ζ_ltに係る旋回角度のプロファイルを示している。また、図７（ｂ）において、太破線の「Follower」は、例えば後続湾曲区間の目標旋回角度ζ_lfに係る旋回角度のプロファイルを示している。

図７（ａ）に関し、基台部１４０の変位ｚ_bが変位ａにおいて、最遠位湾曲区間の目標湾曲角度θ_ltが最遠位湾曲角度ａから最遠位湾曲角度Ｂに変更されると、例えば、太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θ_lfは、基台部１４０の変位ｚ_bが変位ｃにおいて、後続湾曲角度ｃから後続湾曲角度Ｄに変更されるように自動更新される。また、図７（ｂ）に関し、基台部１４０の変位ｚ_bが変位ａにおいて、最遠位湾曲区間の目標旋回角度ζ_ltが最遠位旋回角度ａから最遠位旋回角度ｂに変更されると、例えば、太破線の「Follower」で示す後続湾曲区間の目標旋回角度ζ_lfは、基台部１４０の変位ｚ_bが変位ｃにおいて、後続旋回角度ｃから後続旋回角度ｄに変更されるように自動更新される。ここで、変位ｃは、変位ａと変位ｃの間の長さが後続湾曲区間の長さｌ_dとなるように決定される。

しかしながら、図７（ａ）及び図７（ｂ）において、基台部１４０の変位ｚ_bが変位ａと変位ｃとの間にあるときには、太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfは変化しない。そして、図７（ａ）において太破線の「Follower」で示す後続湾曲区間の目標湾曲角度θ_lfでは、変位ｃにおいて後続湾曲角度ｃから後続湾曲角度Ｄとなり、図７（ｂ）において太破線の「Follower」で示す後続湾曲区間の目標旋回角度ζ_lfでは、変位ｃにおいて後続旋回角度ｃから後続旋回角度ｄとなるため、連続体ロボット１００は急峻な挙動を示し、連続体ロボット１００の操作性が損なわれる。

そこで、本実施形態では、図４のブロックＦＴＬは、以下のようにして、後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLを算出する。
まず、図７（ａ）の目標湾曲角度θについては、図４のブロックＦＴＬは、変位ａと変位ｃとの間では変位ａにおける目標湾曲角度ａと変位ｃにおける目標湾曲角度Ｄとを結ぶ線で補間し、これを後続湾曲区間の目標湾曲角度θ_fFTLとして算出する。図７（ａ）では、この補間した部分を実線で示している。同様に、図７（ｂ）の目標旋回角度ζについては、図４のブロックＦＴＬは、変位ａと変位ｃとの間では変位ａにおける目標旋回角度ａと変位ｃにおける目標旋回角度ｄとを結ぶ線で補間し、これを後続湾曲区間の目標旋回角度ζ_fFTLとして算出する。図７（ｂ）では、この補間した部分を実線で示している。そして、図４のブロックＦＴＬは、算出した後続湾曲区間の目標湾曲角度θ_fFTLに係る湾曲角度のプロファイル及び目標旋回角度ζ_fFTLに係る旋回角度のプロファイルを、例えば内部の記憶部に記憶する。

上述した図７の説明では、最遠位湾曲区間として図１の湾曲区間１７３を適用し、後続湾曲区間として図１の湾曲区間１７２を適用した場合を想定した例について説明した。そして、連続体ロボット１００の湾曲可能部１７０の湾曲区間数が３つ以上、例えば、図１に示す湾曲可能部１７０の湾曲区間数が３つの場合において、上述した後続湾曲区間として図１の湾曲区間１７１を適用する場合には、例えば、上述した最遠位湾曲区間として１つ前の図１の湾曲区間１７２を適用することで実現可能である。

２.２）観察動作制御との切り替え制御
上記「２.１）先頭追従制御」の章で説明した先頭追従制御系を用いることで、操縦者が最遠位湾曲区間に角度指令を与えるのみで、後続の全ての湾曲区間の姿勢が制御される。しかしながら、目標経路６１０の状況により、操縦者が途中で観察動作制御を行うことが必要な場合が考えられる。これは、後続湾曲区間の姿勢に追加指令を加えることで可能となる。例えば、第２の湾曲区間１７２のアクチュエータのみを駆動すると、第３の湾曲区間１７３の姿勢は変化しない。この動作は、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を変化させることができ、例えば胃壁や腸壁などの、連続体ロボット１００の進行方向に対して斜めに走る壁面に沿って、観察する際に適している（以下、「斜視動作」と呼ぶ）。このとき、先端追従制御系による湾曲姿勢に対して操縦者の指令値が追加されるが、例えば、追加指令を与える操作系が中点復帰式のレバーであると、先頭追従制御による湾曲形状が把握しづらく、誤操作により連続体ロボット１００自身やその周囲の対象物に損傷を及ぼす恐れがある。また、操作レバーを離すと、追加の操作量がリセットされてしまう。また、例えば、追加指令を与える操作系が湾曲角速度を与えるものであるとすると、追加の操作量はリセットされないが、結果として与えられる湾曲角度を把握することが直接的ではなく、難しい。これを解決するには、先頭追従制御中には、操作レバーの角度が湾曲角度や旋回角度と同期していることが望ましい。そこで、本実施形態では、操作装置２００－１として、モータ及びエンコーダ等の角度センサを有している操作系を用いる。これにより、操縦者は、先頭追従制御による連続体ロボット１００の姿勢を把握することができ、指令値を追加する場合には、そのまま操作レバーに操作トルクを与えればよい。

ここで再び、図４の説明を行う。
以下に、図４に示す連続体ロボットの制御システム３００－１と、上記「１）モデリング」で示したモデル及び上記「２.１）先頭追従制御」の章で説明した先頭追従制御系との関係を説明する。

図４を用いて上述したように、ブロックＰ_sは、制御対象である連続体ロボット１００、ブロックＰ_sからの出力であるｌ_pは、ワイヤの駆動変位を示すベクトル、ブロックＫ_sは、ワイヤの目標駆動変位ｌ_prefに整定させるための位置制御系である。また、ブロックＰ_lは、（４）式に示したモータを含む操作系であり、ブロックＫ_lは、それを目標値に整定させるための位置制御系であり、この位置制御系の入力端及び出力端に接続されるブロックＧ_in及びＧ_outは、０から１の値をとるゲインである。なお、上述したように、本実施形態では、最遠位湾曲区間を操作するための操作部に含まれる操作レバーは、モータ駆動されない。

また、最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltは、例えば、最遠位湾曲区間の湾曲角度及び旋回角度の操作レバーの角度であり、（４）式におけるθ_ln及びζ_lnと等しい。また、後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfは、例えば、後続湾曲区間の湾曲角度及び旋回角度の操作レバーの角度であり、（４）式における［θ_l1，...，θ_ln-1，ζ_l1，...，ζ_ln-1］からなるベクトルである。また、上述したように、操作トルクｄ_θlf及びｄ_ζlfは、操縦者が後続湾曲区間の湾曲角度及び旋回角度の操作レバーに印加する操作トルクを表し、（４）式における［ｄ_θ1，...，ｄ_θn-1，ｄ_ζ1，...，ｄ_ζn-1］からなるベクトルである。

上述したように、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態となっている。そして、このときには、運動学演算部３４０には、ＦＴＬブロックから出力される先頭追従制御系のための後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLが入力される。同時に、先頭追従制御系のための後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLは、操作レバーの目標角度となり、また、ブロックＧ_in及びブロックＧ_outの値には１が入力されるため、操作レバーの角度は、ブロックＫ_lによってフィードバック制御されて後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLに追従する。このとき、操作レバーに印加される操作トルクｄ_θlf及びｄ_ζlfは、外乱として抑圧されるため、操作レバーの角度θ_lf及びζ_lfには、ほとんど影響しない。

ここで、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態とは逆の方向に切り替える。このとき、ブロックＧ_in及びブロックＧ_outの値には０が入るため、操作レバーの制御系は遮断され、操作トルクｄ_θlf及びｄ_ζlfにより操作レバーの角度θ_lf及びζ_lfを変動させることができる。そして、このときには、運動学演算部３４０には、操作レバーの角度θ_lf及びζ_lfが入力されるため、操縦者は、連続体ロボット１００を先頭追従制御による姿勢から継ぎ目なく追加の操作を加えることが可能となる。その後、再び、後続湾曲区間の操作レバーへの操縦者の接触が未検知となると（具体的には、第２のスイッチ３３２に１の信号が入力されると）、切替スイッチ部３３０は、図４に示している２極双投形スイッチの状態に切り替える。これにより、連続体ロボット１００及び操作レバーの角度は、先頭追従制御による姿勢へと復帰する。なお、本実施形態では、連続体ロボット１００の姿勢角度と操作レバーの角度とを一致させているが、この態様に限定されるものではなく、例えば倍率を与えてもよい。

３）シミュレーション
本章では、前章「２）制御系設計」で説明した先頭追従制御系を用いてシミュレーションを行う。シミュレーションでは、図１に示す連続体ロボット１００の湾曲可能部１７０のように湾曲区間数が３であり、湾曲区間の長さを０．０１ｍとした連続体ロボット１００を対象とする。

図８は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００－１による連続体ロボット１００の制御方法におけるシミュレーション結果の第１例を示す図である。具体的に、図８（ａ）～図８（ｆ）は、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図８（ａ）～図８（ｆ）は、先頭追従制御と追加操作による制御を行い、基台部１４０が０．０２ｍに進行するまでの湾曲可能部１７０の姿勢を段階的にスティック線図により示したものである。また、図８（ａ）～図８（ｆ）では、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。

まず、図８（ａ）では、操縦者の湾曲操作により、最遠位湾曲区間である第３の湾曲区間１７３が湾曲し、基台部１４０の進行を開始している姿勢を示している。続いて、図８（ｂ）では、操縦者が基台部１４０をさらに進行させると、第２の湾曲区間１７２は、先頭追従制御系により、第３の湾曲区間１７３に追従していることがわかる。

続いて、図８（ｃ）では、操縦者が第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作に基づく動作制御を示している。これにより、切替スイッチ部３３０が図４に示している２極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作を行った姿勢を示している。さらに、図８（ｄ）では、第２の湾曲区間１７２の湾曲角度θが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を変化させる斜視動作ができることがわかる。

続いて、図８（ｅ）では、操縦者による第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作が終了した状態を示し、追加操作の前の姿勢に復帰している。続いて、図８（ｆ）では、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従し、先頭追従制御を継続することが可能であることがわかる。

図９は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００－１による連続体ロボット１００の制御方法におけるシミュレーション結果の第２例を示す図である。具体的に、図９（ａ）～図９（ｆ）は、横方向を図１のｙ方向とし、縦方向を図１のｚ方向とし、奥行き方向を図１のｘ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図９（ａ）～図９（ｆ）では、旋回角度ζに追加操作が加わるシミュレーション応答を示している。

まず、図９（ａ）及び図（ｂ）では、上述した図８（ａ）及び図８（ｂ）と同様に、先頭追従制御系が行われている。

続いて、図９（ｃ）では、操縦者が第２の湾曲区間１７２の旋回角度ζが深くなる方向に操作レバーを操作した場合を示している。これにより、図９（ｃ）では、切替スイッチ部３３０が図４に示している２極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作が行われている姿勢を示している。さらに、図９（ｄ）では、２の湾曲区間１７２の旋回角度ζが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を湾曲形状の面外方向に変化させることができることがわかる。

図９（ｅ）及び図９（ｆ）では、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従し、先頭追従制御を継続することが可能であることがわかる。

図１０は、本発明の第１の実施形態に係る連続体ロボットの制御システム３００による連続体ロボット１００の制御方法におけるシミュレーション結果の第３例を示す図である。具体的に、図１０（ａ）～図１０（ｆ）は、横方向を図１のｙ方向とし、縦方向を図１のｚ方向とし、奥行き方向を図１のｘ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１０（ａ）～図１０（ｆ）では、図８の操作と図９の操作を組み合わせることで、３次元空間内で第３の湾曲区間１７３の先端の方向を一定に保ったまま湾曲可能部１７０の最遠位端の位置を操作している。

以上説明した第１の実施形態に係る連続体ロボットの制御システム３００－１においては、ブロックＦＴＬでは、最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltと、基台部１４０の変位とに基づいて、後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLを算出するようにしている。そして、切替スイッチ部３３０では、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLと、後続操作手段に相当するブロックＰ_lから得られる後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfとのうちのいずれか一方を選択するための切替処理を行うようにしている。そして、運動学演算部３４０では、切替スイッチ部３３０で選択された後続湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で後続湾曲区間のワイヤを駆動させる際の駆動変位を演算するようにしている。
かかる構成によれば、先頭追従制御に基づく後続湾曲区間の動作制御と、後続操作手段からの操作に基づく後続湾曲区間の動作制御とを切り替えることができるため、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。具体的に、本実施形態では、先頭追従制御系から継ぎ目なく観察動作制御に移行し、追加操作による斜視動作を行い、さらに、追加操作を終了すると再び先頭追従制御に復帰可能である。

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

第２の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第２の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。

上述した第１の実施形態で説明した制御系は、最遠位湾曲区間については、操縦者の操作指令のみを与えていたが、第２の実施形態では、最遠位湾曲区間を後続湾曲区間の追加操作と連動させることにより、観察動作の種類を増やす形態を示す。例えば、図１に示す連続体ロボット１００の湾曲可能部１７０のように湾曲区間数が３の場合では、第２の湾曲区間１７２と第３の湾曲区間１７３を同方向に駆動すると、連続体ロボット１００の湾曲可能部１７０の先端位置を大きく変化させることができる。この動作は、例えば、体内を広範囲に渡って観察する際に、或いは臓器との接触を避けるために連続体ロボット１００の最遠位端を大きく移動させる必要がある際に、適している（以下、「大湾曲動作」と呼ぶ）。

図１１は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－２」として説明を行う。また、この図１１において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。具体的に、図１１には、最遠位湾曲区間を後続湾曲区間の追加操作と連動させる制御系のブロック線図を示している。

図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００－２において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、ブロックＫ_lt、ブロックＫ_ltの入力端及び出力端にそれぞれ接続されるブロックＧ_in及びＧ_out、ブロックＰ_lf、ブロックＫ_rが追加されている。また、連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、入力装置３１０が削除される変更がされ、ブロックＦＴＬがブロックＦＴＬ＋Memory（記憶部）に変更され、ブロックＫ_lがブロックＫ_lfに変更され、ブロックＰ_lがブロックＰ_lfに変更され、切替スイッチ部３３０に第３のスイッチ３３３が追加される変更がされている。さらに、連続体ロボットの制御システム３００－２では、図４の連続体ロボットの制御システム３００－１に対して、第２のスイッチ３３２の後段にＮＯＴゲートが追加されている。

図１１において、ブロックＰ_lt及びブロックＰ_lfは、それぞれ、最遠位湾曲区間のモータを含む操作系（最遠位操作手段）及び後続湾曲区間のモータを含む操作系（後続操作手段）である。また、図１１において、ブロックＫ_lt及びＫ_lfは、それぞれ、最遠位湾曲区間の操作系位置制御系及び後続湾曲区間の操作系位置制御系である。また、図１１において、最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltは、それぞれ、最遠位区間の湾曲角度の操作レバーによる角度及び最遠位区間の旋回角度の操作レバーによる角度であり、（４）式におけるθ_ln及びζ_lnに相当する。操作トルクｄ_θlt及びｄ_ζltは、操縦者が最遠位湾曲区間の湾曲角度及び旋回角度の操作レバーに印加する操作トルクを表し、（４）式におけるｄ_θn及びｄ_ζnに相当する。また、ブロックＦＴＬ＋Memory（記憶部）には、第１の実施形態における先頭追従制御に加えて、最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltを記憶する記憶部が備えられている。

ブロックＫ_rは、後続湾曲区間と最遠位湾曲区間の姿勢を連動するアルゴリズムであり、これを切り替えることにより動作の種類を変更することができる。具体的に、ブロックＫ_rは、ブロックＦＴＬ＋Memory（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltと、後続操作手段に相当するブロックＰ_lfから入力された後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfとに基づいて、最遠位湾曲区間の目標湾曲角度及び目標旋回角度を算出する最遠位算出手段である。

また、切替スイッチ部３３０の第３のスイッチ３３３は、ブロックＫ_rから得られる最遠位湾曲区間の目標湾曲角度及び目標旋回角度と、最遠位操作手段であるブロックＰ_ltから得られる最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltとのうちのいずれか一方を選択するための切替処理を行うスイッチ（最遠位切替手段）である。

そして、本実施形態では、運動学演算部３４０は、第３のスイッチ３３３で選択された最遠位湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の駆動変位を演算する形態を採る。

本実施形態では、切替スイッチ部３３０は、先頭追従制御が行われているときには（具体的には、第２のスイッチ３３２に１の信号が入力されているときには）、図１１に示している３極双投形スイッチの状態となっている。そして、この場合、第２のスイッチ３３２の後段に設けられているＮＯＴゲートによって、図１１の上方に記載されている最遠位湾曲区間の操作系のフィードバックループにおけるブロックＧ_in及びＧ_outには０が入力される。これにより、最遠位湾曲区間の操作系は遮断され、操作トルクｄ_θlt及びｄ_ζltによって操作レバーの角度θ_lt及びζ_ltを変動させることができる。即ち、ここで説明した先頭追従制御が行われているときには、第３のスイッチ３３３は、ブロックＰ_ltから得られる最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltを選択するための切替処理を行う。そして、この場合、運動学演算部３４０には、ブロックＰ_ltから得られる最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltと、ブロックＦＴＬから得られる後続湾曲区間の目標湾曲角度θ_fFTL及び目標旋回角度ζ_fFTLが入力される。

ここで、後続湾曲区間の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図１１に示している３極双投形スイッチの状態とは逆の方向に切り替える。この場合、図１１の下方に記載されている後続湾曲区間の操作系のフィードバックループにおけるブロックＧ_in及びＧ_outには０が入力される。これにより、後続湾曲区間の操作系は遮断され、操作トルクｄ_θlf及びｄ_ζlfによって操作レバーの角度θ_lf及びζ_lfを変動させることができる。同時に、運動学演算部３４０には、第１のスイッチ３３１を介して後続湾曲区間の目標湾曲角度θ_lf及び目標旋回角度ζ_lfが入力されるため、操縦者は、連続体ロボット１００を先頭追従制御による姿勢から継ぎ目なく追加の操作を加えることが可能となる。一方、第２のスイッチ３３２の後段に設けられているＮＯＴゲートによって、図１１の上方に記載されている最遠位湾曲区間の操作系のフィードバックループにおけるブロックＧ_in及びＧ_outには１が入力される。これにより、ブロックＫ_ltによる位置決め制御が有効となり、操縦者の操作トルクｄ_θlt及びｄ_ζltは外乱として抑圧される。そして、この場合、第３のスイッチ３３３は、ブロックＫ_rから得られる最遠位湾曲区間の目標湾曲角度及び目標旋回角度を選択するための切替処理を行う。これにより、連続体ロボット１００と操作レバーの双方ともに追加の操作量に連動し、最遠位湾曲区間に係る角度は、後続湾曲区間の追加操作量に連動することになる。

図１２は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００－２による連続体ロボット１００の制御方法におけるシミュレーション結果の第１例を示す図である。具体的に、図１２（ａ）～図１２（ｆ）は、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１２（ａ）～図１２（ｆ）は、追加操作が加わるシミュレーション応答を示している。また、図１２（ａ）～図１２（ｆ）では、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。

図１２（ａ）及び図１２（ｂ）では、上述した図９（ａ）及び図９（ｂ）と同様の先頭追従制御系が行われている。

続いて、図１２（ｃ）では、操縦者が第２の湾曲区間１７２の湾曲角度θにおける操作レバーの操作に基づく動作制御を示している。これにより、切替スイッチ部３３０が図１１に示している３極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作を行った姿勢を示している。このシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度との差分θ_l2－θ_2FTLに対して、最遠位湾曲区間の湾曲角度θ_3refが、以下の（８）式となるように連動させる。

ここで、θ_3Memは、ブロックＦＴＬ＋Memory（記憶部）に記憶されている、切替スイッチ部３３０の３極双投形スイッチが切り替わる直前の最遠位湾曲区間の湾曲角度である。

さらに、図１２（ｄ）では、第２の湾曲区間１７２の湾曲角度θが浅くなる方向に追加操作が行われている姿勢を示している。これにより、第３の湾曲区間１７３は、常に前方の［ｘ，ｙ］＝［０．０１５，０．４］付近の狭い領域を向きながら、観察方向が追加操作により変更されていることがわかる。

続いて、図１２（ｅ）及び図１２（ｆ）では、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。

図１３は、本発明の第２の実施形態に係る連続体ロボットの制御システム３００－２による連続体ロボット１００の制御方法におけるシミュレーション結果の第２例を示す図である。具体的に、図１３（ａ）～図１３（ｆ）は、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１３（ａ）～図１３（ｆ）は、追加操作による大湾曲動作を示している。

この図１３に示すシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度の差分に対して、最遠位端の湾曲角度θ_3refが、以下の（９）式になるように連動させる。

これにより、第２の湾曲区間１７２と第３の湾曲区間１７３が連動し、大きな湾曲動作を実現していることわかる。図１３（ｅ）及び図１３（ｆ）では、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。

なお、図１２及び図１３に示すシミュレーションでは、ｘｚ平面における湾曲可能部１７０の動作制御を想定したため、湾曲角度θを考慮した説明を行ったが、ｘｙｚの３次元空間の場合には、更に旋回角度ζも考慮した態様を採りうる。

第２の実施形態によれば、第１の実施形態と同様に、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。

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

第３の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第３の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。

上述した第２の実施形態で説明した制御系は、最遠位湾曲区間を後続湾曲区間の追加操作と連動させた形態であったが、第３の実施形態では、後続湾曲区間同士を連動させることにより、観察動作の種類を増やす形態を示す。

図１４は、本発明の第３の実施形態に係る連続体ロボットの制御システム３００の概略構成の一例を示す図である。以下の説明では、図１４に示す第３の実施形態に係る連続体ロボットの制御システム３００を「連続体ロボットの制御システム３００－３」として説明を行う。また、この図１４において、図４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。具体的に、図１４には、後続湾曲区間同士を追加操作により連動させる制御系のブロック線図を示している。

図１４に示す第３の実施形態に係る連続体ロボットの制御システム３００－３において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、後続湾曲区間の制御系が２つの制御系に分割されて構成されたものである。具体的には、図４に示すブロックＫ_l、ブロックＫ_lの入力端及び出力端にそれぞれ接続されるブロックＧ_in及びＧ_out及びブロックＰ_lを含む後続湾曲区間の制御系が、第２の湾曲区間１７２（第２の後続湾曲区間）の制御系として変更されている。より詳細に、第２の湾曲区間１７２の制御系は、図１４に示すブロックＫ_l2、ブロックＫ_l2の入力端及び出力端にそれぞれ接続されるブロックＧ_in及びＧ_out及びブロックＰ_l2を含む後続湾曲区間の制御系である。また、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、第１の湾曲区間１７１（第１の後続湾曲区間）の制御系が追加されている。より詳細に、第１の湾曲区間１７１の制御系は、図１４に示すブロックＫ_l1及びブロックＰ_l1を含む後続湾曲区間の制御系である。さらに、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、制御ゲインブロックＧ_wが追加されている。さらに、連続体ロボットの制御システム３００－３では、図４の連続体ロボットの制御システム３００－１に対して、切替スイッチ部３３０において第１のスイッチ３３１に替えて第４のスイッチ３３４及び第５のスイッチ３３５が設けられている。

ブロックＰ_l1は、第１の湾曲区間１７１（第１の後続湾曲区間）の目標湾曲角度θ_lf1及び目標旋回角度ζ_lf1を入力するモータを有する操作系（第１の後続操作手段）である。このブロックＰ_l1は、例えば、図３に示す例では、後続湾曲区間である第１の湾曲区間１７１を操作するための第１の操作部２１０に相当する操作系である。具体的に、第１の湾曲区間１７１の目標湾曲角度θ_lf1及び目標旋回角度ζ_lf1は、第１の湾曲区間１７１の操作レバーによる角度である。また、操作トルクｄ_θlf1及びｄ_ζlf1は、それぞれ、操縦者が、第１の湾曲区間１７１の湾曲角度における操作レバー及び第１の湾曲区間１７１の旋回角度における操作レバーに印加する操作トルクを表している。
ブロックＰ_l2は、第２の湾曲区間１７２（第２の後続湾曲区間）の目標湾曲角度θ_lf2及び目標旋回角度ζ_lf2を入力するモータを有する操作系（第２の後続操作手段）である。このブロックＰ_l2は、例えば、図３に示す例では、後続湾曲区間である第２の湾曲区間１７２を操作するための第２の操作部２２０に相当する操作系である。具体的に、第２の湾曲区間１７２の目標湾曲角度θ_lf2及び目標旋回角度ζ_lf2は、第２の湾曲区間１７２の操作レバーによる角度である。また、操作トルクｄ_θlf2及びｄ_ζlf2は、それぞれ、操縦者が、第２の湾曲区間１７２の湾曲角度における操作レバー及び第２の湾曲区間１７２の旋回角度における操作レバーに印加する操作トルクを表している。

ブロックＫ_l1は、第１の湾曲区間１７１（第１の後続湾曲区間）の操作系位置制御系であり、また、ブロックＫ_l2は、第２の湾曲区間１７２（第２の後続湾曲区間）の操作系位置制御系である。

本実施形態においては、ブロックＦＴＬは、第２の湾曲区間１７２の目標湾曲角度θ_f2FTL及び目標旋回角度ζ_f2FTLを算出するとともに、第１の湾曲区間１７１の目標湾曲角度θ_f1FTL及び目標旋回角度ζ_f1FTLを算出する。

図１４に示す連続体ロボットの制御システム３００－３において、制御ゲインブロックＧ_wは、後続湾曲区間と最遠位湾曲区間の姿勢の比率を指定する行列であり、これを切り替えることにより動作の種類を変更できる。具体的に、図１４に示す例では、制御ゲインブロックＧ_wは、第２の後続操作手段であるブロックＰ_l2から得られる第２の湾曲区間１７２の目標湾曲角度θ_lf2及び目標旋回角度ζ_lf2に対して制御ゲインを乗ずる制御ゲイン手段である。

本実施形態では、例えば第２の湾曲区間１７２の操作レバーへの操縦者の接触が検知されると（具体的には、第２のスイッチ３３２に０の信号が入力されると）、切替スイッチ部３３０は、図１４に示している３極双投形スイッチの状態とは逆の方向に切り替える。これにより、切替スイッチ部３３０は、第４のスイッチ３３４において、第２の湾曲区間１７２の目標湾曲角度及び目標旋回角度として、第２の後続操作手段であるブロックＰ_l2から得られる第２の湾曲区間１７２の目標湾曲角度θ_lf2及び目標旋回角度ζ_lf2を選択するための切替処理を行うとともに、第５のスイッチ３３５において、第１の湾曲区間１７１の目標湾曲角度及び目標旋回角度として制御ゲインブロックＧ_wによって制御ゲインを乗じた第２の湾曲区間１７２の目標湾曲角度θ_lf2及び目標旋回角度ζ_lf2を選択するための切替処理を行う。

図１５は、本発明の第３の実施形態に係る連続体ロボットの制御システム３００－３による連続体ロボット１００の制御方法におけるシミュレーション結果の一例を示す図である。具体的に、図１５（ａ）～図１５（ｆ）は、横方向を図１のｘ方向とし、縦方向を図１のｚ方向としており、連続体ロボット１００の湾曲可能部１７０における湾曲区間１７１～１７３の動作制御を時間経過の順に示している。より詳細に、図１５（ａ）～図１５（ｆ）は、追加操作が加わるシミュレーション応答を示している。また、図１５（ａ）～図１５（ｆ）では、実線で連続体ロボット１００の湾曲可能部１７０の形状を示し、丸点で各湾曲区間の先端を示し、細線で各湾曲区間の先端の軌跡を示している。

まず、図１５（ａ）及び図１５（ｂ）では、上述した図９（ａ）及び図９（ｂ）と同様の先頭追従制御系が行われている。

続いて、図１５（ｃ）及び図１５（ｄ）では、操縦者が第２の湾曲区間１７２の湾曲角度の操作レバーを操作した場合を示している。これにより、図１５（ｃ）及び図１５（ｄ）では、切替スイッチ部３３０が図１４に示している３極双投形スイッチの状態とは逆の方向に切り替わり、先頭追従制御の姿勢に対して追加の操作が行われている姿勢を示している。このシミュレーションでは、第２の湾曲区間１７２の先頭追従制御の指令角度と追加操作による角度の差分θ_l2－θ_2FTLに対して、第１の湾曲区間１７１の湾曲角度θ_1refが以下の（１０）式となるように連動させる。

これにより、第３の湾曲区間１７３は、先端の方向は一定のままで、操縦者は、進行方向の前後方向に最遠位端の変位を変更できることがわかる。

続いて、図１５（ｅ）及び図１５（ｆ）では、さらに基台部１４０が進行し、第２の湾曲区間１７２及び第１の湾曲区間１７１は、それぞれ、第３の湾曲区間１７３及び第２の湾曲区間１７２に追従する先頭追従制御を継続することが可能であることがわかる。

なお、図１５に示すシミュレーションでは、ｘｚ平面における湾曲可能部１７０の動作制御を想定したため、湾曲角度θを考慮した説明を行ったが、ｘｙｚの３次元空間の場合には、更に旋回角度ζも考慮した態様を採りうる。

第３の実施形態によれば、第１の実施形態と同様に、意図せずに連続体ロボット１００が対象物と接触してしまうことを回避することができる。これにより、対象物が傷ついたり連続体ロボット自体が破損したりすることを防止することができ、連続体ロボットの操作を安全に行うことができる。

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

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

上述した第１～第３の実施形態は、先頭追従制御を伴う前進時における追加湾曲操作を想定した形態であった。第４の実施形態では、後退時の追加湾曲操作を想定した形態について説明する。ここで、後退時は、最遠位湾曲区間の湾曲角度θ及び旋回角度ζは、前進時と同様に操縦者が操作してもよいが、前進時の操作を記録しておき、それを基台部１４０の変位ｚ_bに応じて再現してもよい。

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

図１６に示す第４の実施形態に係る連続体ロボットの制御システム３００－４において、図１１に示す第２の実施形態に係る連続体ロボットの制御システム３００－２に対して、追加・変更された構成部は、以下のとおりである。
連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、「Ｂｋ＝１ Ｆｗ＝０」ブロック３５０及び切替スイッチ部３６０が追加されている。また、連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、ブロックＫ_rが削除される変更がされている。また、連続体ロボットの制御システム３００－４では、図１１の連続体ロボットの制御システム３００－２に対して、切替スイッチ部３３０の第３のスイッチ３３３が第６のスイッチに変更され、さらに、第２のスイッチ３３２の後段のＮＯＴゲートが乗算ブロック３７０に変更されている。

「Ｂｋ＝１ Ｆｗ＝０」ブロック３５０は、基台部１４０の変位ｚ_bに基づいて、基台部１４０が前進しているか後進しているかを判定する基台部判定手段であり、判定の結果、前進時は０の信号を出力し、後進時は１の信号を出力する。

切替スイッチ部３６０は、「Ｂｋ＝１ Ｆｗ＝０」ブロック３５０の判定結果に基づき、ブロックＦＴＬ＋Memory（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度及び目標旋回角度と、最遠位操作手段であるブロックＰ_ltから得られる最遠位湾曲区間の目標湾曲角度θ_lt及び目標旋回角度ζ_ltとのうちのいずれか一方を選択するための切替処理を行う最遠位切替手段である。具体的に、切替スイッチ部３６０は、「Ｂｋ＝１ Ｆｗ＝０」ブロック３５０の出力信号と連動している。例えば、切替スイッチ部３６０は、「Ｂｋ＝１ Ｆｗ＝０」ブロック３５０によって基台部１４０が後進していると判定された場合（１の信号が出力された場合）、ブロックＦＴＬ＋Memory（記憶部）に記憶されている最遠位湾曲区間の目標湾曲角度及び目標旋回角度を選択するための切替処理を行う。そして、運動学演算部３４０は、この切替スイッチ部３６０で選択された最遠位湾曲区間の目標湾曲角度及び目標旋回角度に基づいて、連続体ロボット１００の駆動部で最遠位湾曲区間のワイヤを駆動させる際の駆動変位を演算する。

具体的に、図１６に示している切替スイッチ部３３０の３極双投形スイッチの状態及び切替スイッチ部３６０の１極双投形スイッチの状態では、最遠位湾曲区間の操作レバーの位置決め制御は無効化される。そして、この場合、操縦者による操作レバーの操作角度が、切替スイッチ部３６０の１極双投形スイッチを介して運動学演算部３４０に入力されるため、先頭追従制御が行われる。ここで、基台部１４０が後進（後退）すると、切替スイッチ部３６０が図１６に示している１極双投形スイッチの状態とは逆の方向に切り替わる。この場合、最遠位湾曲区間の操作レバーの位置決め制御が有効化され、その目標値は、ＦＴＬ＋Memory（記憶部）から前進時に記録された基台部１４０の変位ｚ_bに応じた最遠位湾曲区間の目標湾曲角度及び目標旋回角度が出力される。この最遠位湾曲区間の目標湾曲角度及び目標旋回角度は、同時に、運動学演算部３４０に入力されるため、連続体ロボット１００は、前進時に記録された形状を後進（後退）しながら再現する。

後続湾曲区間の操作レバーへの操縦者の接触が検知されると、本実施形態の制御系は、切替スイッチ部３３０が図１６に示している３極双投形スイッチの状態とは逆の方向に切り替える。この時の後続湾曲区間の動作は、上述した第１の実施形態と同様である。最遠位湾曲区間の制御については、基台部１４０の変位ｚ_bが前進時であっても後進時であっても、最遠位湾曲区間の操作レバーの位置決め制御は無効化され、操縦者による操作レバーの操作角度が、切替スイッチ部３６０を介さずに切替スイッチ部３３０を介して、運動学演算部３４０に入力される。

第４の実施形態によれば、後進時に最遠位湾曲区間に対して前進時の操作記録を再現するように制御しながら、第1の実施形態と同様に、操縦者の追加操作へと継ぎ目なく遷移が可能である。

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

第５の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、本発明の第５の実施形態に係る連続体ロボットの制御システムの概略構成は、上述した第１～第４の実施形態に係る連続体ロボットの制御システム３００－１～３００－４の概略構成のいずれも適用可能である。

上述した第１～第４の実施形態では、図３に示す、各湾曲区間に対する湾曲角度及び旋回角度に１対１で対応する操作レバーを有する操作装置２００－１を用いることを想定した形態であった。第５の実施形態では、操作装置２００として２軸のモータ付きジョイスティックを用いる形態を示す。

図１７は、本発明の第５の実施形態を示し、図１に示す連続体ロボット１００の３つの湾曲区間１７１～１７３を操作するための操作装置２００の概略構成の一例を示す図である。以下の説明では、図１７に示す第５の実施形態に係る操作装置２００を「操作装置２００－５」として説明を行う。

操作装置２００－５は、図１７に示すように、第１の湾曲区間１７１を操作するための第１の操作部２４０、第２の湾曲区間１７２を操作するための第２の操作部２５０、及び、第３の湾曲区間１７３を操作するための第３の操作部２６０を有して構成されている。

第１の操作部２４０は、操作レバー２４１、モータ２４２及び２４４、並びに、角度センサ２４３及び２４５を含み構成されている。操作レバー２４１は、第１の湾曲区間１７１の操作レバーである。角度センサ２４３及び２４５は、それぞれ、第１の湾曲区間１７１に対する操作角度φ_x1及びφ_y1を検出する。モータ２４２及び２４４は、それぞれ、操作角度φ_x1及びφ_y1に制御トルクを与えるためのモータである。

第２の操作部２５０は、操作レバー２５１、モータ２５２及び２５４、並びに、角度センサ２５３及び２５５を含み構成されている。操作レバー２５１は、第２の湾曲区間１７２の操作レバーである。角度センサ２５３及び２５５は、それぞれ、第２の湾曲区間１７２に対する操作角度φ_x2及びφ_y2を検出する。モータ２５２及び２５４は、それぞれ、操作角度φ_x2及びφ_y2に制御トルクを与えるためのモータである。

第３の操作部２６０は、操作レバー２６１、モータ２６２及び２６４、並びに、角度センサ２６３及び２６５を含み構成されている。操作レバー２６１は、第３の湾曲区間１７３の操作レバーである。角度センサ２６３及び２６５は、それぞれ、第３の湾曲区間１７３に対する操作角度φ_x3及びφ_y3を検出する。モータ２６２及び２６４は、それぞれ、操作角度φ_x3及びφ_y3に制御トルクを与えるためのモータである。

上述した第１～第４の実施形態で示した制御系で用いるためには、第ｎの湾曲区間に対する操作角度をφ_xn及びφ_ynとすると、以下の（１１）式に示すように座標変換すればよい。

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

第６の実施形態に係る連続体ロボットの概略構成は、図１及び図２に示す第１の実施形態に係る連続体ロボット１００の概略構成と同様である。また、第６の実施形態に係る操作装置の概略構成は、図３に示す第１の実施形態に係る操作装置２００－１の概略構成と同様である。

上述した第１～第５の実施形態で示した制御系は、先頭追従制御と追加操作の切り替えをするために、ブロックＧ_in及びＧ_outに０の値または１の値を用いていた。しかしながら、このような２値による切り替えは、操作レバーの急峻な挙動を招くことが懸念されるため、以下の図１８に示す制御系を適用することも可能である。

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

図１８に示す第６の実施形態に係る連続体ロボットの制御システム３００－６において、図４に示す第１の実施形態に係る連続体ロボットの制御システム３００－１に対して、追加・変更された構成部は、以下のとおりである。
連続体ロボットの制御システム３００－６では、図４の連続体ロボットの制御システム３００－１に対して、切替スイッチ部３３０の第２のスイッチ３３２における後段にローパスフィルタＦ_lpfを追加している。この図１８に示す第６の実施形態に係る連続体ロボットの制御システム３００－６では、ローパスフィルタＦ_lpfを介した信号がブロックＧ_in及びＧ_outに入力されることになる。この際、ローパスフィルタＦ_lpfの折点周波数は、１Ｈｚ程度とすればよい。図１８では、第１の実施形態の制御系に対する変更点を説明したが、第２～第４の実施形態の制御系についても同様に変更してもよい。

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

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

１００：連続体ロボット、１４０：基台部、１７０：湾曲可能部、１７１～１７３：湾曲区間、３００：連続体ロボットの制御システム、３１０：入力装置、３２０：入力装置、３３０：切替スイッチ部、３４０：運動学演算部

Claims (9)

1. ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出手段と、
   操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作手段と、
   前記後続算出手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替手段と、
   前記後続切替手段で選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算手段と、
   を有することを特徴とする連続体ロボットの制御システム。
2. 前記後続切替手段は、前記後続操作手段への操縦者の接触が検知された場合、前記後続操作手段から得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行うことを特徴とする請求項１に記載の連続体ロボットの制御システム。
3. 前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を記憶する記憶手段と、
   前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作手段から入力された前記後続湾曲区間の目標湾曲角度および目標旋回角度と、に基づいて、前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を算出する最遠位算出手段と、
   操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段と、
   前記最遠位算出手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う最遠位切替手段と、
   を更に有し、
   前記演算手段は、更に、前記最遠位切替手段で選択された前記最遠位湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記最遠位湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算し、
   前記最遠位切替手段は、前記後続操作手段への操縦者の接触が検知された場合、前記最遠位算出手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
   ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
4. 前記後続湾曲区間は、少なくとも２つの後続湾曲区間を備えており、
   前記後続操作手段は、前記２つの後続湾曲区間のうちの第１の後続湾曲区間の目標湾曲角度および目標旋回角度を入力する第１の後続操作手段と、前記２つの後続湾曲区間のうちの前記第１の後続湾曲区間とは異なる第２の後続湾曲区間の目標湾曲角度および目標旋回角度を入力する第２の後続操作手段と、を備えており、
   前記第２の後続操作手段から得られる前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度に対して制御ゲインを乗ずる制御ゲイン手段を更に有し、
   前記後続切替手段は、前記第２の後続操作手段への操縦者の接触が検知された場合、前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度として前記後続操作手段から得られる前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行うとともに、前記第１の後続湾曲区間の目標湾曲角度および目標旋回角度として前記制御ゲイン手段によって前記制御ゲインを乗じた前記第２の後続湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
   ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
5. 前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を記憶する記憶手段と、
   前記基台部の変位に基づいて、前記基台部が前進しているか後進しているかを判定する基台部判定手段と、
   操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段と、
   前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う最遠位切替手段と、
   を更に有し、
   前記演算手段は、更に、前記最遠位切替手段で選択された前記最遠位湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記最遠位湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算し、
   前記最遠位切替手段は、前記基台部判定手段によって前記基台部が後進していると判定された場合、前記記憶手段に記憶されている前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を選択するための切替処理を行う
   ことを特徴とする請求項１または２に記載の連続体ロボットの制御システム。
6. 操作されることによって前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を入力する最遠位操作手段を更に有し、
   前記後続算出手段は、前記最遠位操作手段から得られる前記最遠位湾曲区間の目標湾曲角度および目標旋回角度を用いて、前記後続湾曲区間の目標湾曲角度および目標旋回角度を算出することを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
7. 前記後続操作手段および前記最遠位操作手段は、モータと、前記モータにより駆動される操作レバーと、角度センサとを含み構成されていることを特徴とする請求項３、５、６のいずれか１項に記載の連続体ロボットの制御システム。
8. ワイヤが駆動することによって湾曲する湾曲区間が複数設けられた湾曲可能部と、前記湾曲可能部を支える基台部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御方法であって、
   複数の前記湾曲区間のうち、前記基台部から最も遠い位置にある最遠位湾曲区間の目標湾曲角度および目標旋回角度と、前記連続体ロボットの進行方向における前記基台部の変位と、に基づいて、前記複数の湾曲区間のうち、前記最遠位湾曲区間と前記基台部との間に位置する後続湾曲区間の目標湾曲角度および目標旋回角度を算出する後続算出ステップと、
   操作されることによって前記後続湾曲区間の目標湾曲角度および目標旋回角度を入力する後続操作ステップと、
   前記後続算出ステップで得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、前記後続操作ステップで得られる前記後続湾曲区間の目標湾曲角度および目標旋回角度と、のうちのいずれか一方を選択するための切替処理を行う後続切替ステップと、
   前記後続切替ステップで選択された前記後続湾曲区間の目標湾曲角度および目標旋回角度に基づいて、前記駆動部で前記後続湾曲区間の前記ワイヤを駆動させる際の駆動変位を演算する演算ステップと、
   を有することを特徴とする連続体ロボットの制御方法。
9. コンピュータを、請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システムにおける各手段として機能させるためのプログラム。

Images