JP7183062B2 - 連続体ロボット制御装置、連続体ロボット制御方法及びプログラム - Google Patents

Description

本発明は、ワイヤを駆動することにより湾曲可能な湾曲可能部を有する連続体ロボットの動作を制御する連続体ロボット制御装置及び連続体ロボット制御方法、並びに、当該連続体ロボット制御装置としてコンピュータを機能させるためのプログラムに関する。

近年、患者の負担を低減し治療・検査後のＱＯＬを向上させるための低侵襲医療が注目を集めている。低侵襲医療の代表例として、内視鏡を用いた手術・検査が挙げられる。例えば、腹腔鏡手術は、従来の開腹手術と比べて手術創を小さくすることが可能となるため、術後に必要な入院期間を短縮できるだけでなく、美容上も優れているというメリットがある。

また、低侵襲医療に用いられる内視鏡として、軟性内視鏡が知られている。この軟性内視鏡は、挿入部が湾曲可能な部材で構成されているため、例えば、食道や大腸、肺などの湾曲する器官であっても組織を圧迫することなく体内の深部に到達し、患者の負担を低減できるものである。さらに、アクチュエータなどを用いて挿入部を駆動し、体内の経路に沿うようにその姿勢を制御すれば、患者の負担をさらに低減できることが期待される。そのため、軟性内視鏡として利用可能な連続体ロボットの機構とその制御方法の研究開発が盛んに行われている。

このような連続体ロボットでは、例えば、ワイヤ等の駆動力伝達機構を用いることでアクチュエータを基台に設置し、湾曲可能部を細径化する方式が用いられることがある。例えば、特許文献１には、駆動力伝達機構としてワイヤを用いる連続体ロボットにおいて、ワイヤの駆動量を制御することで湾曲可能部の姿勢を目標姿勢に一致させる方法が記載されている。この際、特許文献１では、ワイヤが長手方向に剛であると仮定し、運動学を用いてワイヤの駆動量を演算している。

米国特許第９３３３６５０号明細書

肺のような体内深部の狭小空間に到達可能な連続体ロボットを実現するためには、駆動伝達機構であるワイヤは、長尺かつ細径であることが望ましい。しかしながら、このようなワイヤは、押し引きの際に生じる張力により大きく変形する。これにより、例えば、特許文献１に記載の技術のように、運動学を用いて演算したワイヤの駆動量を制御入力とすると、湾曲可能部の目標姿勢と実際の姿勢との間に誤差が生じ、体内の腔壁と湾曲可能部との接触リスクが増加するという問題があった。

本発明は、このような問題点に鑑みてなされたものであり、腔壁（体内等の経路）と湾曲可能部との接触リスクの低減を実現する仕組みを提供することを目的とする。

本発明の連続体ロボット制御装置は、複数のワイヤのうちの少なくとも一部のワイヤを駆動することにより湾曲する湾曲可能部を有する連続体ロボットの動作を制御する連続体ロボット制御装置であって、前記湾曲可能部の湾曲角度の目標値である目標湾曲角度および前記湾曲可能部の旋回角度の目標値である目標旋回角度に基づいて、前記少なくとも一部のワイヤの駆動量を演算する第１の演算手段と、前記目標湾曲角度と、前記目標旋回角度と、前記目標湾曲角度および前記目標旋回角度における前記複数のワイヤのうちのいずれかのワイヤの伸縮量とに基づいて、前記駆動量を補償する補償量を演算する第２の演算手段と、前記第１の演算手段で算出された駆動量と、前記第２の演算手段で算出された補償量とに基づいて、前記少なくとも一部のワイヤを駆動制御する駆動制御量を設定する設定手段と、を有する。
また、本発明は、上述した連続体ロボット制御装置による連続体ロボット制御方法、及び、上述した連続体ロボット制御装置における各手段としてコンピュータを機能させるためのプログラムを含む。

本発明によれば、体内等の経路と湾曲可能部との接触リスクの低減を実現することができる。

本発明の第１の実施形態で用いる連続体ロボットの外観構成の一例を示す図である。 本発明の第１の実施形態を示し、図１に示す湾曲可能部の概略構成の一例を示す図である。 本発明の第１の実施形態を示し、図２に示す湾曲可能部における運動学モデルの一例を示す図である。 本発明の第１の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第１の実施形態を示し、目標湾曲角度と角度誤差との関係を示す特性図である。 本発明の第１の実施形態を示し、ワイヤ長と角度誤差との関係を示す特性図である。 本発明の第２の実施形態で用いる連続体ロボットの概略構成の一例を示す図である。 本発明の第２の実施形態を示し、図７に示す第１の湾曲可能部及び第２の湾曲可能部における運動学モデルの一例を示す図である。 本発明の第２の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第２の実施形態を示し、図９に示す補償量演算部の機能構成の一例を示す図である。 本発明の第２の実施形態を示し、図１０に示す伸縮量更新部の機能構成の一例を示す図である。 本発明の第２の実施形態を示し、反復計算の試行数である反復数と伸縮量との関係を示す特性図である。 本発明の第２の実施形態を示し、第１の湾曲可能部及び第２の湾曲可能部における目標湾曲角度と角度誤差との関係を示す特性図である。 本発明の第３の実施形態で用いる連続体ロボットの概略構成の一例を示す図である。 本発明の第３の実施形態を示し、図１４に示す湾曲可能部における運動学モデルの一例を示す図である。 本発明の第３の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第３の実施形態を示し、図１６に示す補償量演算部の機能構成の一例を示す図である。 本発明の第３の実施形態を示し、図１７に示す伸縮量更新部の機能構成の一例を示す図である。 本発明の第３の実施形態を示し、目標湾曲角度と湾曲角度の誤差との関係を示す特性図である。 本発明の第３の実施形態を示し、目標旋回角度と湾曲角度の誤差との関係を示す特性図である。 本発明の第４の実施形態で用いる連続体ロボットの概略構成の一例を示す図である。 本発明の第４の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第５の実施形態に係る連続体ロボット制御システムの外観構成の一例を示す図である。 本発明の第５の実施形態を示し、図２３に示す湾曲可能部における運動学モデルの一例を示す図である。 本発明の第５の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第６の実施形態で用いる連続体ロボットの概略構成の一例を示す図である。 本発明の第６の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第６の実施形態を示し、図２７に示す補償量演算部の機能構成の一例を示す図である。 本発明の第６の実施形態を示し、図２８に示す伸縮量更新部の機能構成の一例を示す図である。 本発明の第６の実施形態を示し、目標旋回角度と湾曲角度の誤差との関係を示す特性図である。 本発明の第７の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第７の実施形態を示し、目標湾曲角度と駆動制御量との関係を示す特性図である。 本発明の第７の実施形態を示し、目標湾曲角度と湾曲角度の誤差との関係を示す特性図である。

以下に、本発明の各実施形態について、図面を参照して詳細に説明する。具体的に、以下に説明する本発明の各実施形態では、連続体ロボット（連続体マニピュレータともいう）の制御システムが可撓性内視鏡に適用される例を示す。なお、本発明の各実施形態に係る連続体ロボット制御システムの例として適用する可撓性内視鏡は、医療分野に限定されるものではなく、湾曲可能部を挿入・抜去させる経路の内部を観察するものであれば他の分野（例えば、配管等の内部を観察するための工業用内視鏡）にも適用可能である。

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

＜１－１．モデリング＞
図１は、本発明の第１の実施形態で用いる連続体ロボット１１０の外観構成の一例を示す図である。この図１に示す連続体ロボット１１０は、ワイヤ１ａ、ワイヤ１ｂ、長尺部１１１、湾曲可能部１１２、アクチュエータ１１３、及び、固定部１１４を有して構成されている。

長尺部１１１は、外力に対して受動的に湾曲する構成部である。湾曲可能部１１２は、複数のワイヤ１ａ及び１ｂのうちの一部のワイヤ１ｂ（所定ワイヤ）を駆動することにより湾曲可能な構成部である。アクチュエータ１１３は、ワイヤ１ｂを駆動する構成部である。固定部１１４は、ワイヤ１ａを固定する構成部である。

ここで、以下の説明においては、連続体ロボット１１０の各構成部において、基台の面（例えば、長尺部１１１におけるアクチュエータ１１３及び固定部１１４側の端部の面）から遠い側の端部を「遠位端」と称し、上述した基台の面から近い側の端部を「近位端」と称する。

図２は、本発明の第１の実施形態を示し、図１に示す湾曲可能部１１２の概略構成の一例を示す図である。ここで、図２には、湾曲可能部１１２を挿入・抜去させる経路の一例も図示している。

湾曲可能部１１２は、当該湾曲可能部１１２の近位端の面を介して延伸する複数のワイヤ１ａ及び１ｂと、複数のワイヤ１ａ及び１ｂが異なる位置に固定され、複数のワイヤ１ａ及び１ｂを案内する第１のワイヤガイド１１２１と、上述した近位端の面と第１のワイヤガイド１１２１との間に設けられ、複数のワイヤ１ａ及び１ｂを案内する第２のワイヤガイド１１２２～１１２５と、を含み形成されている。また、図２には、湾曲可能部１１２の遠位端１１２６と、上述した湾曲可能部１１２の近位端の面における中心位置１１２７を図示している。具体的に、ワイヤ１ａとワイヤ１ｂは、各ワイヤガイド１１２１～１１２５と長尺部１１１に設けられた孔に案内され、その一端は第１のワイヤガイド１１２１に固定されるとともに、ワイヤ１ａの他端は固定部１１４に接続され、ワイヤ１ｂの他端はアクチュエータ１１３に接続される。そして、図１に示すアクチュエータ１１３を湾曲可能部１１２の長手方向に駆動するとワイヤ１ｂが押引きされ、湾曲可能部１１２が湾曲する。

図３は、本発明の第１の実施形態を示し、図２に示す湾曲可能部１１２における運動学モデルの一例を示す図である。

図３では、図２に示す湾曲可能部１１２の近位端の面における中心位置１１２７を原点とし、長手方向にＺ₁軸をとり、円周方向にＸ₁軸をとり、紙面の手前から奥に向かってＹ₁軸をとる。同様に、図２に示す湾曲可能部１１２の遠位端１１２６の面における中心位置を原点とし、長手方向にＺ₂軸をとり、円周方向にＸ₂軸をとり、紙面の手前から奥に向かってＹ₂軸をとる。また、図３に示すように、Ｚ₁軸とＺ₂軸とのなす角を湾曲角度θ₁とする。

また、以降の説明においては、必要に応じて以下に定義する符号を用いた説明を行う。
θ₁：湾曲可能部の遠位端の絶対湾曲角度
ｌ_1a1，ｌ_1b1：湾曲可能部のワイヤ１ａの長さ，ワイヤ１ｂの長さ
ｌ_p1b：湾曲可能部のワイヤ１ｂの駆動制御量
ｌ_e1b：湾曲可能部のワイヤ１ｂの補償量
Δｌ_1a，Δｌ_1b：湾曲可能部のワイヤ１ａの伸縮量，ワイヤ１ｂの伸縮量
ｆ_1a，ｆ_1b：湾曲可能部のワイヤ１ａから遠位端に作用する力，ワイヤ１ｂから遠位端に作用する力
Ｍ_1a1,Ｍ_1b1：湾曲可能部のワイヤ１ａから遠位端に作用する曲げモーメント，ワイヤ１ｂから遠位端に作用する曲げモーメント
Ｒ_g：湾曲可能部の中心からワイヤ１ａ，ワイヤ１ｂまでの距離を半径とする円の直径
Ｌ₁₀：湾曲可能部のワイヤ１ａ，ワイヤ１ｂの全長
ｌ₁₀：湾曲角度が０（ｄｅｇ）のときの湾曲可能部のワイヤ長
Ｅ：ワイヤのヤング率
Ａ：ワイヤの断面積
Ｉ：ワイヤの断面２次モーメント
なお、駆動制御量ｌ_p1b、伸縮量Δｌ_1a，Δｌ_1b、力ｆ_1a，ｆ_1bは、遠位端方向を正とし、また、モーメントＭ_1a1，Ｍ_1b1は、時計方向を正とし、以下の実施形態においても同様である。

また、本実施形態では、以下に記載する仮定を前提とした運動学モデルと弾性ワイヤ駆動モデルを考える。

［仮定１］：湾曲可能部において、ワイヤは曲率一定に変形する。
［仮定２］：ワイヤのねじり変形を考慮しない。
［仮定３］：ワイヤとワイヤガイドとの間、及び、ワイヤと長尺部の間に生じる摩擦を考慮しない。
［仮定４］：ワイヤガイドとワイヤの間に働く力とモーメントを考慮しない。
［仮定５］：ワイヤと遠位端の間に働く力のうち、ワイヤの長手方向の成分のみを考慮し、半径方向の成分を考慮しない。
［仮定６］：ワイヤの伸縮量は、ワイヤに作用する張力に比例する。
［仮定７］：ワイヤの曲げモーメントは、たわみ角に比例する。
［仮定８］：各ワイヤの引っ張り剛性と曲げ剛性とは等しい。
［仮定９］：各ワイヤのヤング率、断面積、断面２次モーメントは等しい。

まず、ワイヤ１ａ及びワイヤ１ｂが長手方向に剛であると仮定し、運動学モデルを表現する関係式を導出する。ここで、図３に示すように、ワイヤ１ａとワイヤ１ｂとの距離をＲ_gとすると、上述した仮定１及び仮定２から、湾曲角度θ₁は、湾曲可能部１１２におけるワイヤ１ａの長さｌ_1a1とワイヤ１ｂの長さｌ_1b1を用いて、以下の（１）式で表される。

ここで、（１）式の右辺は、ワイヤ１ｂの駆動量と等しいため、運動学モデルを用いて演算すると、ワイヤ１ｂの駆動量ｌ_k1bは、以下の（２）式で表される。

次に、ワイヤの伸縮を考慮して弾性ワイヤ駆動モデルを表現する関係式を導出する。
湾曲角度θ₁が０（ｄｅｇ）のときの湾曲可能部１１２のワイヤ長をｌ₁₀とし、ワイヤ１ａの伸縮量をΔｌ_1a、ワイヤ１ｂの伸縮量をΔｌ_1bとし、弾性ワイヤ駆動モデルを用いて演算するワイヤ１ｂの駆動制御量をｌ_p1bとすると、ワイヤ長ｌ_1a1及びワイヤ長ｌ_1b1は、それぞれ、以下の（３）式及び（４）式で表される。

そして、（２）式～（４）式を（１）式に代入すると、ワイヤ１ｂの駆動制御量ｌ_p1bは、以下の（５）式で表される。

即ち、（５）式から、ワイヤ１ｂの駆動制御量ｌ_p1bは、ワイヤ１ａ及び１ｂの伸縮量における差分と駆動量ｌ_k1bとの和になることがわかる。したがって、補償量ｌ_e1bを以下の（６）式とすれば、ワイヤの伸縮による駆動量の誤差を補償することができる。

次に、湾曲可能部１１２の遠位端１１２６と各ワイヤに作用する力及びモーメントのつり合いの式から、ワイヤ１ａの伸縮量Δｌ_1a及びワイヤ１ｂの伸縮量Δｌ_1bを導出する。上述した仮定３、仮定４及び仮定５から、湾曲可能部１１２の遠位端１１２６には、ワイヤ１ａ及びワイヤ１ｂから受ける長手方向の力ｆ_1a及びｆ_1bと、Ｙ₁軸周りの曲げモーメントＭ_1a1及びＭ_1b1が作用する。したがって、湾曲可能部１１２の遠位端１１２６における力とモーメントのつり合いは、それぞれ、以下の（７）式及び（８）式で表される。

また、上述した仮定６、仮定７及び仮定８から、力ｆ_1a，ｆ_1b及びモーメントＭ_1a1，Ｍ_1b1は、伸縮量Δｌ_1a及びΔｌ_1bと湾曲角度θ₁を用いて、それぞれ、以下の（９）式及び（１０）式で表される。

この（９）式及び（１０）式において、定数ｋ_eと定数ｋ_mは、それぞれ、ワイヤの引っ張り剛性と曲げ剛性を表す。そして、この定数ｋ_e及び定数ｋ_mは、上述した仮定９から、ワイヤの全長（ワイヤ長）Ｌ₀、ワイヤの断面積Ａ、断面２次モーメントＩ、ヤング率Ｅを用いると、それぞれ、以下の（１１）式及び（１２）式で表すことができる。

また、（７）式と（９）式により、本実施形態では、ワイヤ１ｂの伸縮量Δｌ_1bとワイヤ１ａの伸縮量Δｌ_1aとの間に、以下の（１３）式の関係が成立する。

このため、（６）式と（１３）式により、ワイヤ１ｂの補償量ｌ_e1bは、以下の（１４）式で表すことができる。

そして、（９）式と（１０）式を（８）式に代入し、さらに、（３）式、（４）式、（１３）式、（１４）式を用いてｌ_1a1，ｌ_1b1，Δｌ_1b，ｌ_p1bを消去すると、以下の（１５）式に示すワイヤ１ａの伸縮量Δｌ_1aの多項式を得る。

（１５）式において、Ａ₃～Ａ₀及びＢ₁～Ｂ₀は、湾曲角度θ₁を変数とするワイヤ１ａの伸縮量Δｌ_1aの係数であり、それぞれ、以下の（１６）式及び（１７）式で表される。

そして、（１５）式の分子は、ワイヤ１ａの伸縮量Δｌ_1aについて３次の多項式であるため、これを解くと、３つの異なる解を得る。本実施形態では、この３つの解のうち、絶対値が最も小さい解をワイヤ１ａの伸縮量Δｌ_1aとする。

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

連続体ロボット制御装置１２０は、入力装置１３０から入力された、湾曲可能部１１２の湾曲角度の目標値である目標湾曲角度θ_1refに基づいて、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1bを設定し、連続体ロボット１１０（より具体的には、アクチュエータ１１３）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1bを出力する。

この図４に示す連続体ロボット制御装置１２０は、運動学演算部１２１、補償量演算部１２２、加算部１２３、及び、位置制御部１２４を有して構成されている。

運動学演算部１２１は、入力装置１３０から入力された湾曲可能部１１２の目標湾曲角度θ_1refと、ワイヤ１ｂ（所定ワイヤ）とワイヤ１ａ（他のワイヤ）との距離とに基づいて、ワイヤ１ｂの駆動量ｌ_k1bを演算する第１の演算手段である。具体的に、運動学演算部１２１は、（２）式の湾曲角度θ₁に目標湾曲角度θ_1refを代入して、ワイヤ１ｂの駆動量ｌ_k1bを算出する。

補償量演算部１２２は、入力装置１３０から入力された湾曲可能部１１２の目標湾曲角度θ_1refと、ワイヤ１ａの伸縮量Δｌ_1aとに基づいて、ワイヤ１ｂの補償量ｌ_e1bを演算する第２の演算手段である。具体的に、補償量演算部１２２は、（１５）式を用いてワイヤ１ａの伸縮量Δｌ_1aを算出し、（１４）式を用いてワイヤ１ａの伸縮量Δｌ_1aからワイヤ１ｂの補償量ｌ_e1bを算出する。

加算部１２３は、運動学演算部１２１で算出された駆動量ｌ_k1bと、補償量演算部１２２で算出された補償量ｌ_e1bとを加算して、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1bを算出する。即ち、加算部１２３は、運動学演算部１２１で算出された駆動量ｌ_k1bを補償量演算部１２２で算出された補償量ｌ_e1bで補償する処理を行って、ワイヤ１ｂの駆動制御量ｌ_p1bを算出する。また、位置制御部１２４は、連続体ロボット１１０（より具体的には、アクチュエータ１１３）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1bを出力する。ここで、本実施形態においては、加算部１２３及び位置制御部１２４は、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1bを設定する設定手段を構成する。

＜１－３．シミュレーション＞
上記＜１－１＞で導出した弾性ワイヤ駆動モデルと、上記＜１－２＞で提示した制御系を用いてシミュレーションを行う。本実施形態では、湾曲可能部１１２の長さｌ₀を０．０１ｍとする。

まず、ワイヤ１ａ，ワイヤ１ｂの全長（ワイヤ長）Ｌ₀を１ｍとし、本実施形態の制御系を適用することで湾曲可能部１１２の角度誤差を低減可能であることを以下に説明する。

図５は、本発明の第１の実施形態を示し、目標湾曲角度と角度誤差との関係を示す特性図である。図５では、横軸に目標湾曲角度（Reference bending angle(deg)）をとり、縦軸に角度誤差（Angular displacement(deg)）をとった特性図を示している。

具体的に、図５では、目標湾曲角度θ_1refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させるときの角度誤差を示している。この図５において、実線は、弾性ワイヤ駆動モデルを用いて演算したワイヤ１ｂの駆動制御量ｌ_p1bを入力したときの応答の角度誤差を示し、破線は、運動学モデルを用いて演算したワイヤ１ｂの駆動量ｌ_k1bを入力したときの応答の角度誤差を示している。この図５に示す特性から、本実施形態に相当する駆動制御量ｌ_p1bを用いる場合には、目標湾曲角度θ_1refが変化しても角度誤差が常に０となることがわかる。一方、比較例に相当する駆動量ｌ_k1bを用いる場合には、目標湾曲角度θ_1refが増減すると角度誤差が増加してしまうことがわかる。このことから、本実施形態に係る図４に示す制御系を適用することにより、連続体ロボット１１０の姿勢誤差を低減可能なことがわかる。

次に、湾曲可能部１１２の長さと比べて長尺のワイヤを備える連続体ロボット１１０の姿勢制御において、本実施形態に係る図４に示す制御系が特に有効であることを以下に説明する。

図６は、本発明の第１の実施形態を示し、ワイヤ長Ｌ₀と角度誤差との関係を示す特性図である。図６では、横軸にワイヤ長（Wire length）Ｌ₀をとり、縦軸に角度誤差（Angular displacement(deg)）をとった特性図を示している。

具体的に、図６は、湾曲可能部１１２の長さｌ₀を０．０１ｍとする本実施形態において、目標湾曲角度θ_1refを９０（ｄｅｇ）とし、ワイヤ長Ｌ₀を０．０１ｍから１ｍとする連続体ロボット１１０に駆動量ｌ_k1bを入力したときの湾曲角度の誤差を示している。この図６に示す特性から、ワイヤ長Ｌ₀を０．１ｍ以上とすると、大きな角度誤差が生じてしまうことがわかる。このことから、本実施形態に係る図４に示す制御系は、ワイヤ長Ｌ₀が、湾曲可能部１１２の長さｌ₀の１０倍以上となる連続体ロボット１１０において特に効果を発揮する。

第１の実施形態に係る連続体ロボット制御装置１２０では、補償量演算部１２２において、入力装置１３０から入力された湾曲可能部１１２の目標湾曲角度θ_1refと、ワイヤ１ａの伸縮量Δｌ_1aとに基づいて、ワイヤ１ｂの補償量ｌ_e1bを演算するようにしている。そして、加算部１２３及び位置制御部１２４において、運動学演算部１２１で算出された駆動量ｌ_k1bを補償量演算部１２２で算出された補償量ｌ_e1bで補償する処理を行って、ワイヤ１ｂの駆動制御量ｌ_p1bを設定するようにしている。
かかる構成によれば、ワイヤの変形に起因する駆動量ｌ_k1bの誤差を補償することができる。これにより、湾曲可能部１１２の目標姿勢と実際の姿勢との誤差を減少させることができ、その結果、体内等の経路と湾曲可能部との接触リスクの低減を実現することができる。

本実施形態の補償量演算部１２２は、ワイヤ１ａ（他のワイヤ）の伸縮量Δｌ_1aを用いて補償量ｌ_e1bを導出するが、ワイヤ１ｂ（所定ワイヤ）の伸縮量Δｌ_1bを用いて補償量ｌ_e1bを導出してもよい。具体的には、（１３）式を用いて（１５）式のΔｌ_1aを－Δｌ_1bに置き換えた、以下の（１８）式を導出し、（１８）式を用いてΔｌ_1bを演算する。そして、（１４）式のΔｌ_1aを－Δｌ_1bに置き換えた、以下の（１９）式を用いて補償量ｌ_e1bを演算する。

なお、以下の実施形態では、他のワイヤの伸縮量から補償量を演算する方法のみを示すが、本実施形態と同様に所定ワイヤの伸縮量を用いて補償量を演算することも可能である。

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

第２の実施形態は、経路に対して挿入・抜去させる方向に複数（具体的には、２つ）の湾曲可能部を有する連続体ロボットを適用する形態である。

第１の実施形態における（１５）式は、ワイヤの伸縮量について３次の多項式となるため、解析解を容易に求めることができる。しかしながら、複数の湾曲可能部を有する連続体ロボットでは、（１５）式に対応する多項式における伸縮量の最高次数が５次以上となるため、解析解を求めることは困難である。そこで、第２の実施形態における制御系は、反復計算により伸縮量の数値解を求める。これにより、複数の湾曲可能部を有する連続体ロボットに本発明における制御系を適用することが可能となる。また、反復計算では、弾性ワイヤ駆動モデルより導出する複数の方程式に数値を順に代入し、ワイヤの伸縮量を更新する。これにより、湾曲可能部の数が増加しても、簡単な更新則を組み合わせることで反復計算を行うことが可能となる。

以下では、まず、連続体ロボットの運動学モデルと弾性ワイヤ駆動モデルを導出する。続いて、反復計算を用いてワイヤの伸縮量を演算する本実施形態における制御系を提示し、複数の湾曲可能部を有する連続体ロボットにおいても制御性能を向上可能であることを数値例を用いて説明する。

＜２－１．モデリング＞
図７は、本発明の第２の実施形態で用いる連続体ロボット２１０の概略構成の一例を示す図である。この図７において、図１及び図２に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。また、この図７では、湾曲可能部１１２及び２１２を挿入・抜去させる経路の記載は省略している。

具体的に、この図７に示す連続体ロボット２１０は、図２に示す湾曲可能部１１２に相当する第１の湾曲可能部１１２、及び、第２の湾曲可能部２１２を有して構成されている。即ち、この図７に示す連続体ロボット２１０は、互いに直列に設けられた複数の湾曲可能部１１２及び２１２を有して構成されている。

第２の湾曲可能部２１２は、当該第２の湾曲可能部２１２の近位端の面（第１の湾曲可能部１１２の遠位端１１２６の面）を介して延伸する複数のワイヤ２ａ及び２ｂと、複数のワイヤ２ａ及び２ｂが異なる位置に固定され、複数のワイヤ２ａ及び２ｂを案内する第１のワイヤガイド２１２１と、上述した近位端の面と第１のワイヤガイド２１２１との間に設けられ、複数のワイヤ２ａ及び２ｂを案内する第２のワイヤガイド２１２２～２１２５と、を含み形成されている。

また、第１の湾曲可能部におけるワイヤガイド１１２１～１１２５には、例えば、ワイヤ１ａとワイヤ１ｂの案内孔の外側に、ワイヤ２ａとワイヤ２ｂを案内するための孔が設けられている。そして、第１の実施形態における連続体ロボット１１０と同様に、各ワイヤは長尺部１１１に案内され、ワイヤ１ａ及びワイヤ２ａは固定部２１４に接続され、ワイヤ１ｂ及びワイヤ２ｂはアクチュエータ２１３に接続される。なお、図７に示す例では、アクチュエータ２１３は、ワイヤ１ｂ及びワイヤ２ｂをそれぞれ独立して駆動することができる駆動機構であるものとし、また、本実施形態においては、ワイヤ１ｂ及びワイヤ２ｂに対応した２つのアクチュエータ２１３を構成する形態であってもよい。

このため、本実施形態では、ワイヤ１ｂを駆動することで第１の湾曲可能部１１２を駆動させることが可能となり、また、ワイヤ２ｂを駆動することで第２の湾曲可能部２１２を駆動させることが可能となる。

以下に示す第２の実施形態では、上述した第１の実施形態で示した仮定１～９に、以下の仮定１０～１２を加えて、上述した第１の実施形態と同様の方法を用いて、第２の実施形態の連続体ロボット２１０における運動学モデルと弾性ワイヤ駆動モデルを導出する。

［仮定１０］：ワイヤ１ａの案内孔とワイヤ２ａの案内孔との距離は短いため、各案内孔と湾曲可能部の中心軸との距離をどちらもＲ_g／２とする。ワイヤ１ｂの案内孔とワイヤ２ｂの案内孔との関係についても同様である。
［仮定１１］：第１の湾曲可能部１１２におけるワイヤ２ａの長さl_2a1とワイヤ１ａの長さl_1a1は等しい。ワイヤ２ｂの長さl_2b1とワイヤ１ｂの長さl_1b1についても同様である。
［仮定１２］：第１の湾曲可能部１１２の遠位端１１２６とワイヤ２ａ及びワイヤ２ｂとの間にはワイヤの曲げモーメントのみが働き、摩擦力と反力は働かない。

図８は、本発明の第２の実施形態を示し、図７に示す第１の湾曲可能部１１２及び第２の湾曲可能部２１２における運動学モデルの一例を示す図である。

図８では、図７に示す第２の湾曲可能部２１２の遠位端２１２６の面における中心位置を原点とし、遠位端２１２６の面の法線方向にＺ₃軸をとり、円周方向にＸ₃軸をとり、紙面手前から奥に向かってＹ₃軸をとる。また、図８に示すように、Ｚ₁軸とＺ₃軸とのなす角を絶対湾曲角度θ₂とし、Ｚ₂軸とＺ₃軸とのなす角を相対湾曲角度~θ₂とする。ここで、湾曲角度~θ₂は、第２の湾曲可能部２１２の基準面が第２の湾曲可能部２１２の基準面と異なることから、相対座標系であることを示す符号（～）を付したものである。また、以下に示す数式では、相対座標系であることを示す符号（～）を各要素の上に付しているが、本明細書に記載の文章中では、書式の関係から湾曲角度「~θ₂」等のように横にずらした記載としており、以降の説明では、これらは同じものを示しているものとする。

本実施形態では、このとき、相対湾曲角度~θ₂は、湾曲角度θ₁及びθ₂を用いて、以下の（２０）式で表される。

また、第１の実施形態における（１）式と同様に、相対湾曲角度~θ₂は、第２の湾曲可能部２１２におけるワイヤ２ａの長さｌ_2a2とワイヤ２ｂの長さｌ_2b2を用いて、以下の（２１）式で表される。

したがって、ワイヤを長手方向に剛と仮定するときの第２の湾曲可能部２１２の駆動量ｌ_k2bは、以下の（２２）式で表される。

また、本実施形態における連続体ロボット２１０の第１の湾曲可能部１１２において（２）式が成立するため、運動学モデルは、（２）式と（２２）式を用いて表すことができる。

また、ワイヤの伸縮を考慮すると、第１の湾曲可能部１１２と第２の湾曲可能部２１２におけるワイヤ２ａ及びワイヤ２ｂの長さは、ワイヤ２ａの伸縮量Δｌ_2a及びワイヤ２ｂの伸縮量Δｌ_2bと、ワイヤ２ｂの駆動制御量ｌ_p2bを用いて、それぞれ、以下の（２３）式及び（２４）式で表される。

ここで、上述した仮定１１から、長さｌ_1a1と長さｌ_2a1、長さｌ_1b1と長さｌ_2b1が等しいため、（３）式を（２３）式に代入し、（４）式を（２４）式に代入すると、長さｌ_2a2及びｌ_2b2は、それぞれ、以下の（２５）式及び（２６）式で表される。

（２２）式と（２５）式と（２６）式を（２１）式に代入し、さらに、（５）式を用いて駆動制御量ｌ_p1bを消去すると、以下の（２７）式を得る。

したがって、第１の実施形態における（６）式と同様に、第２の湾曲可能部２１２のワイヤ２ｂの補償量ｌ_e2bを以下の（２８）式とすれば、第２の湾曲可能部２１２のワイヤの伸縮による駆動量の誤差を補償することができる。

なお、上述したように、第２の実施形態における第１の湾曲可能部１１２の運動学は、上述した第１の実施形態と等しいため、ワイヤ１ｂの駆動制御量ｌ_p1b及び補償量ｌ_e1bは、上述した第１の実施形態と同様に、それぞれ、（５）式及び（６）式で表される。

ワイヤの伸縮量を求めるために、第１の湾曲可能部１１２の遠位端１１２６と第２の湾曲可能部２１２の遠位端２１２６に作用する力とモーメントのつり合いの式を導出する。第１の実施形態における（８）式と同様に、第２の湾曲可能部２１２の遠位端２１２６に作用するＹ₃軸周りのモーメントのつり合いは、以下の（２９）式で表される。

また、Ｚ₃軸方向の力のつり合いは、以下の（３０）式で表される。

なお、モーメントＭ_2a2，Ｍ_2b2及び力ｆ_2a，ｆ_2bは、それぞれ、（１０）式及び（９）式と同様に、以下の（３１）式及び（３２）式で表すことができる。

ここで、（３０）式と（３２）式により、以下の（３３）式が成立する。

よって、（３３）式を（２８）式に代入すると、以下の（３４）式を得る。

そして、（１４）式と（３４）式により、ワイヤ１ａの伸縮量Δｌ_1aとワイヤ２ａの伸縮量Δｌ_2aが求まれば、ワイヤ１ａの補償量ｌ_e1bとワイヤ２ａの補償量ｌ_e2bを演算可能であることがわかる。上述した仮定１２から、第１の湾曲可能部１１２の遠位端１１２６には、第１の湾曲可能部１１２におけるワイヤ１ａ、ワイヤ１ｂ、ワイヤ２ａ及びワイヤ２ｂの曲げモーメントに加えて、モーメントＭ_2a2及びＭ_2b2の反モーメントが作用する。したがって、第１の湾曲可能部１１２の遠位端１１２６におけるＹ₂軸周りのモーメントのつり合いは、以下の（３５）式で表される。

なお、上述した仮定１１から、ワイヤ長ｌ_1a1とワイヤ長ｌ_2a1とが等しく、また、ワイヤ長ｌ_1b1とワイヤ長ｌ_2b1とが等しい。このため、モーメントＭ_2a1とモーメントＭ_1a1とは等しく、また、モーメントＭ_2a1とモーメントＭ _1b1とは等しくなり、以下の（３６）式が成り立つ。

したがって、（３５）式は、以下の（３７）式に変形することができる。

また、上述した仮定１２から、第１の湾曲可能部１１２の遠位端１１２６のＺ₂軸方向には、ワイヤ１ａ及びワイヤ１ｂの張力のみ作用するため、第１の湾曲可能部１１２の力のつり合いの式は、第１の実施形態と同様に、（７）式で表される。

＜２－２．制御系＞
第２の実施形態における弾性ワイヤ駆動モデルに対して、第１の実施形態と同様に式変形を行うことで、ワイヤ１ａの伸縮量Δｌ_1a及びワイヤ２ａの伸縮量Δｌ_2aを変数とする２つの独立な多項式を得ることができる。しかしながら、これらの方程式は、伸縮量Δｌ_1a及びΔｌ_2aについて５次以上の高次の多項式となるため解析解を求めることは困難である。この際、勾配法等の最適化法により数値解を求めることは可能であるが、多数の湾曲可能部を有する連続体ロボット２１０においてこの高次多項式を導出することは容易ではない。そこで、第２の実施形態では、弾性ワイヤ駆動モデルから導出する簡単な式を組み合わせる反復計算により、ワイヤ１ａの伸縮量Δｌ_1a及びワイヤ２ａの伸縮量Δｌ_2aの数値解を求める。

図９は、本発明の第２の実施形態に係る連続体ロボット制御システム２００の機能構成の一例を示す図である。この図９に示す連続体ロボット制御システム２００は、図７に示す連続体ロボット２１０、連続体ロボット制御装置２２０、及び、入力装置２３０を有して構成されている。

第２の実施形態では、入力装置２３０から連続体ロボット制御装置２２０に対して、第１の湾曲可能部１１２の目標湾曲角度θ_1refに加えて第２の湾曲可能部２１２の目標湾曲角度（目標相対湾曲角度）~θ_2refも入力される。

連続体ロボット制御装置２２０は、入力装置１３０から入力された、第１の湾曲可能部１１２の目標湾曲角度θ_1ref及び第２の湾曲可能部２１２の目標湾曲角度~θ_2refに基づいて、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1b及びワイヤ２ｂを駆動制御する駆動制御量ｌ_p2bを設定し、連続体ロボット１１０（より具体的には、アクチュエータ２１３）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p2bに基づく駆動指令τ_2bを出力する。

この図９に示す連続体ロボット制御装置２２０は、運動学演算部２２１、補償量演算部２２２、加算部２２３、及び、位置制御部２２４を有して構成されている。

運動学演算部２２１は、入力装置１３０から入力された第１の湾曲可能部１１２の目標湾曲角度θ_1refと、ワイヤ１ｂ（所定ワイヤ）とワイヤ１ａ（他のワイヤ）との距離とに基づいて、ワイヤ１ｂの駆動量ｌ_k1bを演算するとともに、入力装置１３０から入力された第２の湾曲可能部２１２の目標湾曲角度~θ_2refと、ワイヤ２ｂ（所定ワイヤ）とワイヤ２ａ（他のワイヤ）との距離とに基づいて、ワイヤ２ｂの駆動量ｌ_k2bを演算する。具体的に、運動学演算部２２１は、（２）式の湾曲角度θ₁に目標湾曲角度θ_1refを代入してワイヤ１ｂの駆動量ｌ_k1bを算出するとともに、（２２）式の湾曲角度~θ₂に目標湾曲角度~θ_2refを代入してワイヤ２ｂの駆動量ｌ_k2bを算出する。

補償量演算部２２２は、入力装置１３０から入力された第１の湾曲可能部１１２の目標湾曲角度θ_1refと、ワイヤ１ａの伸縮量Δｌ_1aとに基づいて、ワイヤ１ｂの補償量ｌ_e1bを演算するとともに、入力装置１３０から入力された第２の湾曲可能部２１２の目標湾曲角度~θ_2refと、ワイヤ２ａの伸縮量Δｌ_2aとに基づいて、ワイヤ２ｂの補償量ｌ_e2bを演算する第２の演算手段である。具体的に、補償量演算部２２２は、（１５）式を用いてワイヤ１ａの伸縮量Δｌ_1aを算出し、（１４）式を用いてワイヤ１ａの伸縮量Δｌ_1aからワイヤ１ｂの補償量ｌ_e1bを算出する。また、補償量演算部２２２は、同様にしてワイヤ２ａの伸縮量Δｌ_2aを算出し、（３４）式を用いてワイヤ２ａの伸縮量Δｌ_2aからワイヤ２ｂの補償量ｌ_e2bを算出する。

加算部２２３は、運動学演算部２２１で算出された駆動量ｌ_k1bと、補償量演算部２２２で算出された補償量ｌ_e1bとを加算して、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1bを算出する。また、加算部２２３は、運動学演算部２２１で算出された駆動量ｌ_k2bと、補償量演算部２２２で算出された補償量ｌ_e2bとを加算して、ワイヤ２ｂを駆動制御する駆動制御量ｌ_p2bを算出する。即ち、加算部２２３は、運動学演算部２２１で算出された駆動量ｌ_k1b及びｌ_k2bのそれぞれを補償量演算部２２２で算出された補償量ｌ_e1b及びｌ_e2bで補償する処理を行って、ワイヤ１ｂの駆動制御量ｌ_p1b及びワイヤ２ｂの駆動制御量ｌ_p2bを算出する。また、位置制御部２２４は、連続体ロボット２１０（より具体的には、アクチュエータ２１３）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p2bに基づく駆動指令τ_2bを出力する。ここで、本実施形態においては、加算部２２３及び位置制御部２２４は、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1b及びワイヤ２ｂを駆動制御する駆動制御量ｌ_p2bを設定する設定手段を構成する。

図１０は、本発明の第２の実施形態を示し、図９に示す補償量演算部２２２の機能構成の一例を示す図である。補償量演算部２２２は、図１０に示すように、伸縮量更新部２２２１、収束判定部２２２２、出力切換部２２２３、及び、増幅部２２２４を含み構成されている。

伸縮量更新部２２２１は、目標湾曲角度θ_1ref及び~θ_2refと、ワイヤ１ａ及び２ｂの伸縮量における第１の伸縮量候補値Δｌ_{1a_prev}及びΔｌ_{2a_prev}との入力に応じて、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}に更新する。この際、伸縮量更新部２２２１は、上述した弾性ワイヤ駆動モデルを用いて、ワイヤ１ａ及び２ｂの伸縮量における第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}の更新を行う。

収束判定部２２２２は、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}が一定値に収束しているか否かを判定する。そして、収束判定部２２２２は、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}が一定値に収束していると判定した場合、出力切換部２２２３に対して、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}をそれぞれ伸縮量Δｌ_1a及び伸縮量Δｌ_2aとして増幅部２２２４に出力させる。また、収束判定部２２２２は、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}が一定値に収束していないと判定した場合、出力切換部２２２３に対して、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}をそれぞれ第１の伸縮量候補値Δｌ_{1a_prev}及びΔｌ_{2a_prev}として伸縮量更新部２２２１に出力させ、反復計算を再度行う。ここで、本実施形態では、収束判定部２２２２及び出力切換部２２２３は、本発明における出力処理部に相当する構成である。

増幅部２２２４は、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}をそれぞれ伸縮量Δｌ_1a及び伸縮量Δｌ_2aとして用いて、補償量ｌ_e1b及びｌ_e2bを算出する補償量算出部である。本実施形態では、（１４）式と（３４）式に示すように、それぞれ、補償量ｌ_e1b及びｌ_e2bは、伸縮量Δｌ_1a及びΔｌ_1bの２倍となる。このため、増幅部２２２４は、第１の湾曲可能部１１２については伸縮量Δｌ_1aに２を乗じて補償量ｌ_e1bを算出し、また、第２の湾曲可能部２１２については伸縮量Δｌ_2aに２を乗じて補償量ｌ_e2bを算出する。

なお、収束判定部２２２２は、所定の反復回数以内に第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}が一定値に収束しないときには、出力切換部２２２３に対して、最後に収束した第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}をそれぞれ伸縮量Δｌ_1a及び伸縮量Δｌ_2aとして増幅部２２２４に出力させる形態をとり得る。これにより、増幅部２２２４で算出される補償量が不連続に変化してしまうことを防止することができる。なお、本実施形態では、反復計算における第１の伸縮量候補値Δｌ_{1a_prev}及びΔｌ_{2a_prev}の初期値を０とするが、最後に収束した伸縮量を初期値として用いてもよい。

図１１は、本発明の第２の実施形態を示し、図１０に示す伸縮量更新部２２２１の機能構成の一例を示す図である。伸縮量更新部２２２１は、図１１に示すように、ワイヤ長演算部２２２１１、モーメント演算部２２２１２、力演算部２２２１３、及び、伸縮量演算部２２２１４を含み構成されている。

ワイヤ長演算部２２２１１は、目標湾曲角度θ_1ref及び~θ_2refと第１の伸縮量候補値Δｌ_{1a_prev}及びΔｌ_{2a_prev}との入力に応じて、第１の湾曲可能部１１２及び第２の湾曲可能部２１２における複数のワイヤ１ａ，１ｂ及びワイヤ２ａ，２ｂごとに、ワイヤ長ｌ_1a1，ｌ_1b1，ｌ_2a2，ｌ_2b2を演算する。具体的に、ワイヤ長演算部２２２１１は、（１）式～（５）式と（２１）式～（２７）式を変形する、以下の（３８）式～（４１）式を用いて、ワイヤ長ｌ_1a1，ｌ_1b1，ｌ_2a2，ｌ_2b2を演算する。

モーメント演算部２２２１２は、目標湾曲角度θ_1ref及び~θ_2refとワイヤ長ｌ_1a1，ｌ_1b1，ｌ_2a2，ｌ_2b2との入力に応じて、複数のワイヤ１ａ，１ｂ及びワイヤ２ａ，２ｂごとに、曲げモーメントＭ_1a1，Ｍ_1b1，Ｍ_2a2，Ｍ_2b2を演算する。具体的に、モーメント演算部２２２１２は、（１０）式及び（３６）式を用いて、曲げモーメントＭ_1a1，Ｍ_1b1，Ｍ_2a2，Ｍ_2b2を演算する。

力演算部２２２１３は、複数のワイヤ１ａ，１ｂ及びワイヤ２ａ，２ｂにおける曲げモーメントＭ_1a1，Ｍ_1b1，Ｍ_2a2，Ｍ_2b2の入力に応じて、ワイヤ１ａ及び２ａ（他のワイヤ）の張力を示す力ｆ_1a及びｆ_2aを演算する。具体的に、力演算部２２２１３は、（２９）式及び（３７）式を用いて、力ｆ_1a及びｆ_2aを演算する。

伸縮量演算部２２２１４は、力ｆ_1a及びｆ_2aの入力に応じて、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}を演算して、伸縮量候補値の更新を行う。具体的に、伸縮量演算部２２２１４は、（９）式及び（３２）式を用いて、第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}を演算して、伸縮量候補値の更新を行う。したがって、本実施形態における更新後の第２の伸縮量候補値Δｌ_{1a_next}及びΔｌ_{2a_next}は、それぞれ、以下の（４２）式及び（４３）式で表される。

ただし、（４２）式と（４３）式は、更新則を説明するために示しており、上述したように、伸縮量更新部２２２１は、弾性ワイヤ駆動モデルより導出する複数の方程式に数値を順に代入し伸縮量を更新する。なお、本実施形態における図９～図１１に示す制御系は、伸縮量を用いて反復計算の収束判定を行うが、ワイヤ長や力、曲げモーメントも反復計算の度に更新されるため、これらのうちの１つを収束判定に用いてもよい。

また、本実施形態においては、第２の湾曲可能部２１２の目標湾曲角度として相対湾曲角度~θ_2refを用いるが、（２０）式に示すように絶対角度と相対角度とは相互に変換可能であるため、絶対湾曲角度を目標湾曲角度として用いてもよい。

＜２－３．シミュレーション＞
２つの湾曲可能部１１２及び２１２を有する連続体ロボット２１０に対して、図９～図１１に示す第２の実施形態における制御系を適用する。なお、第１の湾曲可能部１１２と第２の湾曲可能部２１２の長さを０．０１ｍとし、長尺部１１１の長さを１ｍとする。

図１２は、本発明の第２の実施形態を示し、反復計算の試行数である反復数と伸縮量との関係を示す特性図である。図１２では、横軸に反復数（Iteration number）をとり、縦軸に各試行における伸縮量（Displacement）をとった特性図を示している。

具体的に、図１２では、目標湾曲角度θ_1refを－９０（ｄｅｇ）とするとともに、目標湾曲角度~θ_2refを９０（ｄｅｇ）とし、伸縮量Δl_1a及びΔl_2aの初期値を０とした際に、補償量演算部２２２における伸縮量Δl_1a及びΔl_2aの応答を示している。また、図１２において、実線は伸縮量Δl_1aを示し、破線は伸縮量Δl_2aを示している。この図１２に示すように、伸縮量Δl_1a及びΔl_2aは、どちらも4回目の試行後に一定となることから、反復計算により収束していることがわかる。

図１３は、本発明の第２の実施形態を示し、第１の湾曲可能部１１２及び第２の湾曲可能部２１２における目標湾曲角度と角度誤差との関係を示す特性図である。

具体的に、図１３（ａ）は、目標湾曲角度θ_1refを－９０（ｄｅｇ）とし、目標湾曲角度~θ_2refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させた場合に、第１の湾曲可能部１１２の角度誤差を表している。また、図１３（ｂ）は、目標湾曲角度θ_1refを－９０（ｄｅｇ）とし、目標湾曲角度~θ_2refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させた場合に、第２の湾曲可能部２１２の角度誤差を表している。この図１３（ａ）及び図１３（ｂ）では、横軸に目標湾曲角度（Reference angle）~θ_2refをとり、縦軸に図１３（ａ）では角度誤差（Angle error）θ₁を、図１３（ｂ）では角度誤差（Angle error）~θ₂をとった特性図を示している。

図１３（ａ）及び図１３（ｂ）において、点線は、それぞれ、運動学演算部２２１で演算する駆動量ｌ_k1b及びｌ_k2bを入力とする応答を示している。また、図１３（ａ）及び図１３（ｂ）において、実線は、第２の実施形態における弾性ワイヤ駆動モデルから演算する駆動制御量ｌ_p1b及びｌ_p2bを入力とする応答を示している。

この図１３（ａ）及び図１３（ｂ）に示すように、比較例に相当する駆動量ｌ_k1b及びｌ_k2bを用いる場合には、目標湾曲角度~θ_2refの増減に伴って第１の湾曲可能部１１２及び第２の湾曲可能部２１２の角度誤差がどちらも増加してしまうことがわかる。一方、本実施形態に相当する駆動制御量ｌ_p1b及びｌ_p2bを用いる場合には、目標湾曲角度~θ_2refが変化しても、第１の湾曲可能部１１２及び第２の湾曲可能部２１２の角度誤差が常に０となることがわかる。このことから、本実施形態に係る図９～図１１に示す制御系を適用することにより、複数の湾曲可能部を有する連続体ロボット２１０の姿勢誤差を低減できることがわかる。

第２の実施形態に係る連続体ロボット制御装置２２０では、補償量演算部２２２において、複数の湾曲可能部１１２及び２１２における湾曲可能部ごとに、入力装置２３０から入力された湾曲可能部１１２の目標湾曲角度θ_1ref及び~θ_2refと、ワイヤ１ａ及び２ａの伸縮量Δl_1a及びΔl_2aとに基づいて、それぞれ、ワイヤ１ｂ及び２ｂの補償量ｌ_e1b及びｌ_e2bを演算するようにしている。そして、加算部２２３及び位置制御部２２４において、運動学演算部２２１で算出された駆動量ｌ_k1b及びｌ_k2bのそれぞれを補償量演算部２２２で算出された補償量ｌ_e1b及びｌ_e2bで補償する処理を行って、ワイヤ１ｂ及び２ｂの駆動制御量ｌ_p1b及びｌ_p2bを設定するようにしている。
かかる構成によれば、複数の湾曲可能部を有する連続体ロボット２１０においても、ワイヤの変形に起因する駆動量ｌ_k1b及びｌ_k2bの誤差を補償することができる。これにより、複数の湾曲可能部の目標姿勢と実際の姿勢との誤差を減少させることができ、その結果、体内等の経路と湾曲可能部との接触リスクの低減を実現することができる。

なお、本実施形態においても、第１の実施形態と同様に、同じ湾曲可能部に係る複数のワイヤのうちのいずれかのワイヤの伸縮量に基づいて算出した補償量を用いて、各ワイヤの駆動量を補償することができる。

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

第３の実施形態は、立体的に湾曲可能な湾曲可能部を有する連続体ロボットを適用する形態である。具体的に、第３の実施形態における連続体ロボットは２つのワイヤを駆動制御して１つの湾曲可能部を湾曲させ、制御系は当該２つのワイヤの伸縮量をそれぞれ考慮する補償量を第２の実施形態と同様の反復計算により演算する。これにより、立体的に湾曲可能な湾曲可能部を有する連続体ロボットの制御性能を向上させることが可能となる。

＜３－１．モデリング＞
図１４は、本発明の第３の実施形態で用いる連続体ロボット３１０の概略構成の一例を示す図である。この図１４において、図１及び図２や図７に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。また、この図１４では、湾曲可能部３１２を挿入・抜去させる経路の一例も図示している。

湾曲可能部３１２は、当該湾曲可能部３１２の近位端の面（中心位置が位置３１２７の面）を介して延伸する複数のワイヤ１ａ，１ｂ及び１ｃと、複数のワイヤ１ａ，１ｂ及び１ｃが異なる位置に固定され、複数のワイヤ１ａ，１ｂ及び１ｃを案内する第１のワイヤガイド３１２１と、上述した近位端の面と第１のワイヤガイド３１２１との間に設けられ、複数のワイヤ１ａ，１ｂ及び１ｃを案内する第２のワイヤガイド３１２２～３１２５と、を含み形成されている。

そして、第１の実施形態における連続体ロボット１１０と同様に、各ワイヤは長尺部１１１に案内され、ワイヤ１ａは固定部３１１ａに接続され、ワイヤ１ｂはアクチュエータ３１１ｂに接続され、ワイヤ１ｃはアクチュエータ３１１ｃに接続される。また、本実施形態では、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃは、それぞれ、湾曲可能部３１２の中心軸を重心とする正三角形の頂点に配置される。このとき、ワイヤ１ｂ及びワイヤ１ｃの駆動制御量を設定することにより、任意の平面内で湾曲可能部３１２を湾曲させることが可能となる。例えば、ワイヤ１ｂとワイヤ１ｃを同じ方向に同じ距離だけ駆動する場合、湾曲可能部３１２は、図１４のＸＺ平面内で湾曲する。また、ワイヤ１ｂとワイヤ１ｃを逆方向に同じ距離だけ駆動する場合、湾曲可能部３１２は、ＹＺ平面内で湾曲する。本実施形態においても、連続体ロボット３１０の運動学モデルと弾性ワイヤ駆動モデルを導出する。

図１５は、本発明の第３の実施形態を示し、図１４に示す湾曲可能部３１２における運動学モデルの一例を示す図である。

図１５では、図１４に示す湾曲可能部１１２の近位端の面における中心位置３１２７を原点Ｏ₁とし、ワイヤ１ａの方向にＸ₁軸をとり、湾曲可能部３１２の長手方向にＺ₁軸をとり、Ｘ₁軸とＺ₁軸とに直交する方向にＹ₁軸をとる。また、Ｘ₁Ｙ₁平面内で原点Ｏ₁から湾曲可能部３１２の遠位端３１２６の面の中心点Ｏ₂に向かう方向にＷ₁軸をとり、Ｘ₁軸とＷ₁軸とのなす角度を旋回角度ζ₁とする。さらに、遠位端３１２６の中心軸を原点とし、法線方向にＺ₂軸をとり、Ｚ₂軸に垂直で且つＷ₁Ｚ₁平面上にＸ₂軸をとり、Ｘ₂軸とＺ₂軸に垂直な方向にＹ₂軸をとる。

湾曲角度がθ₁、旋回角度がζ₁のとき、ワイヤ１ａとワイヤ１ｂをＷ₁Ｚ₁平面に投影すると、Ｗ₁Ｚ₁平面におけるワイヤ１ａとワイヤ１ｂとの距離Ｒ_baは、以下の（４４）式で表される。

したがって、第１の実施形態における（１）式と同様に、湾曲角度θ₁と長さｌ_1a1及びｌ_1b1との間には、以下の（４５）式が成立する。

同様に、Ｗ₁Ｚ₁平面におけるワイヤ１ａとワイヤ１ｃとの距離Ｒ_caは、以下の（４６）式で表される。

このため、湾曲角度θ₁と長さｌ_1a1及びｌ_1c1との間に、以下の（４７）式が成立する。

そして、（４５）式と（４７）式から、ワイヤを剛と仮定するときのワイヤ１ｂの駆動量l_k1bとワイヤ１ｃの駆動量l_k1cは、それぞれ、以下の（４８）式及び（４９）式で表される。

次に、ワイヤの伸縮を考慮して弾性ワイヤ駆動モデルを導出する。
本実施形態における連続体ロボット３１０では、ワイヤ長ｌ_1a1及びｌ_1b1、並びに、ワイヤ１ｂの駆動制御量ｌ_p1bは、第１の実施形態と同様に、（３）式、（４）式、（５）式で表される。また、ワイヤ長ｌ_1c1は、ワイヤ長ｌ_1b1と同様に、ワイヤ１ｃの駆動制御量ｌ_p1cを用いて、以下の（５０）式で表される。

このため、ワイヤ１ｃの駆動制御量ｌ_p1cは、以下の（５１）式で表される。

この（５１）式から、本実施形態における連続体ロボット３１０において、運動学モデルから導出する駆動量とワイヤの伸縮量の差分を加算することで、ワイヤの伸縮を考慮する駆動量を演算可能なことがわかる。したがって、ワイヤ１ｃの補償量ｌ_e1cは、以下の（５２）式で表される。

ここで、ワイヤの伸縮量を求めるために、湾曲可能部３１２の遠位端３１２６に作用する力とモーメントのつり合いの式を導出する。湾曲可能部３１２の遠位端３１２６は、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃからＺ₂軸方向の力を受けているため、力のつり合いは、力ｆ_1a，ｆ_1b及びｆ_1cを用いて、以下の（５３）式で表される。

また、湾曲可能部３１２の遠位端３１２６には、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃからＹ₂軸まわりの曲げモーメントＭ_1a1，Ｍ_1b1及びＭ_1c1が作用し、曲げモーメントとワイヤから受ける力ｆ_1a，ｆ_1b及びｆ_1cによって生じるモーメントがつり合っている。したがって、Ｙ₂軸まわりのモーメントのつり合いは、以下の（５４）式で表される。

また、Ｘ₂軸まわりには、力ｆ_1a，ｆ_1b及びｆ_1cによるモーメントが生じるため、つり合いの式は、以下の（５５）式で表される。

なお、ワイヤ１ｃの力とモーメントは、ワイヤ１ａ及びワイヤ１ｂと同様に、それぞれ、以下の（５６）式及び（５７）式で表される。

＜３－２．制御系＞
第３の実施形態では、上述した第２の実施形態と同様に、反復計算を用いて各ワイヤの伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cの数値解を求める。

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

連続体ロボット制御装置３２０は、入力装置３３０から入力された湾曲可能部３１２の目標湾曲角度θ_1ref、及び、入力装置３４０から入力された湾曲可能部３１２の目標旋回角度ζ_1refに基づいて、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1b及びワイヤ１ｃを駆動制御する駆動制御量ｌ_p1cを設定し、連続体ロボット１１０（より具体的には、アクチュエータ３１１ｂ及び３１１ｃ）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p1cに基づく駆動指令τ_1cを出力する。即ち、第３の実施形態では、入力装置３３０からの目標湾曲角度θ_1refの入力に加えて、入力装置３４０からの目標旋回角度ζ_1refの入力が行われる点で、上述した第１の実施形態と異なる。

この図１６に示す連続体ロボット制御装置３２０は、運動学演算部３２１、補償量演算部３２２、加算部３２３、及び、位置制御部３２４を有して構成されている。

運動学演算部３２１は、入力装置３３０から入力された湾曲可能部３１２の目標湾曲角度θ_1refと、入力装置３４０から入力された湾曲可能部３１２の目標旋回角度ζ_1refと、ワイヤ１ｂ及び１ｃ（所定ワイヤ）とワイヤ１ａ（他のワイヤ）との距離とに基づいて、ワイヤ１ｂ及び１ｃごとに駆動量ｌ_k1b及びｌ_k1cを演算する。具体的に、運動学演算部３２１は、（４８）式及び（４９）式を用いて、運動学モデルに基づく駆動量ｌ_k1b及びｌ_k1cを算出する。

補償量演算部３２２は、入力装置３３０から入力された湾曲可能部３１２の目標湾曲角度θ_1refと、入力装置３４０から入力された湾曲可能部３１２の目標旋回角度ζ_1refと、各ワイヤの伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cとに基づいて、ワイヤ１ｂ及び１ｃごとに補償量ｌ_e1b及びｌ_e1cを演算する。具体的に、補償量演算部３２２は、まず反復計算により各ワイヤの伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cを求めて、次に、当該伸縮量の差分である補償量ｌ_e1b及びｌ_e1cを算出する。

加算部３２３は、ワイヤ１ｂ及び１ｃごとに、運動学演算部３２１で算出された駆動量ｌ_k1b及びｌ_k1cと補償量演算部３２２で算出された補償量ｌ_e1b及びｌ_e1cとをそれぞれ加算して、駆動制御量ｌ_p1b及びｌ_p1cを演算する。即ち、加算部３２３は、運動学演算部３２１で算出された駆動量ｌ_k1b及びｌ_k1cのそれぞれを補償量演算部３２２で算出された補償量ｌ_e1b及びｌ_e1cで補償する処理を行って、ワイヤ１ｂの駆動制御量ｌ_p1b及びワイヤ１ｃの駆動制御量ｌ_p1cを算出する。また、位置制御部３２４は、連続体ロボット３１０（より具体的には、アクチュエータ３１１ｂ及び３１１ｃ）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p1cに基づく駆動指令τ_1cを出力する。ここで、本実施形態においては、加算部３２３及び位置制御部３２４は、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1b及びワイヤ１ｃを駆動制御する駆動制御量ｌ_p1cを設定する設定手段を構成する。

図１７は、本発明の第３の実施形態を示し、図１６に示す補償量演算部３２２の機能構成の一例を示す図である。補償量演算部３２２は、図１７に示すように、伸縮量更新部３２２１、収束判定部３２２２、出力切換部３２２３、及び、減算部３２２４を含み構成されている。

伸縮量更新部３２２１は、目標湾曲角度θ_1ref及び目標旋回角度ζ_1refと、各ワイヤ１ａ，１ｂ及び１ｃの伸縮量における第１の伸縮量候補値Δｌ_{1a_prev}，Δｌ_{1b_prev}及びΔｌ_{1c_prev}との入力に応じて、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}に更新する。この際、伸縮量更新部３２２１は、上述した弾性ワイヤ駆動モデルを用いて、各ワイヤ１ａ，１ｂ及び１ｃの伸縮量における第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}の更新を行う。

収束判定部３２２２は、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}が一定値に収束しているか否かを判定する。そして、収束判定部３２２２は、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}が一定値に収束していると判定した場合、出力切換部３２２３に対して、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}をそれぞれ伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cとして減算部３２２４に出力させる。また、収束判定部３２２２は、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}が一定値に収束していないと判定した場合、出力切換部３２２３に対して、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}をそれぞれ第１の伸縮量候補値Δｌ_{1a_prev}，Δｌ_{1b_prev}及びΔｌ_{1c_prev}として伸縮量更新部３２２１に出力させ、反復計算を再度行う。ここで、本実施形態では、収束判定部３２２２及び出力切換部３２２３は、本発明における出力処理部に相当する構成である。

減算部３２２４は、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}をそれぞれ伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cとして用いて、（５２）式等から、補償量ｌ_e1b及びｌ_e1cを算出する補償量算出部である。

なお、収束判定部３２２２は、所定の反復回数以内に第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}が一定値に収束しないときには、出力切換部３２２３に対して、最後に収束した第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}をそれぞれ伸縮量Δｌ_1a，Δｌ_1b及びΔｌ_1cとして減算部３２２４に出力させる形態をとり得る。これにより、減算部３２２４で算出される補償量が不連続に変化してしまうことを防止することができる。なお、本実施形態でも、反復計算における第１の伸縮量候補値Δｌ_{1a_prev}，Δｌ_{1b_prev}及びΔｌ_{1c_prev}の初期値を０とするが、最後に収束した伸縮量を初期値として用いてもよい。

図１８は、本発明の第３の実施形態を示し、図１７に示す伸縮量更新部３２２１の機能構成の一例を示す図である。伸縮量更新部３２２１は、図１８に示すように、ワイヤ長演算部３２２１１、モーメント演算部３２２１２、力演算部３２２１３、及び、伸縮量演算部３２２１４を含み構成されている。

ワイヤ長演算部３２２１１は、目標湾曲角度θ_1refと第１の伸縮量候補値Δｌ_{1a_prev}，Δｌ_{1b_prev}及びΔｌ_{1c_prev}との入力に応じて、湾曲可能部３１２における複数のワイヤ１ａ，１ｂ及び１ｃごとに、ワイヤ長ｌ_1a1，ｌ_1b1及びｌ_1c1を演算する。具体的に、ワイヤ長演算部３２２１１は、（３）式、（５）式、（４８）式～（５１）式を変形する、以下の（５８）式～（６０）式を用いて、ワイヤ長ｌ_1a1，ｌ_1b1及びｌ_1c1を演算する。

モーメント演算部３２２１２は、目標湾曲角度θ_1refと目標旋回角度ζ_1refと各ワイヤのワイヤ長ｌ_1a1，ｌ_1b1及びｌ_1c1との入力に応じて、複数のワイヤ１ａ，１ｂ及び１ｃごとに、曲げモーメントＭ_1a1，Ｍ_1b1及びＭ_1c1を演算する。具体的に、モーメント演算部３２２１２は、（１０）式と（５７）式を用いて、曲げモーメントＭ_1a1，Ｍ_1b1及びＭ_1c1を演算する。

力演算部３２２１３は、複数のワイヤ１ａ，１ｂ及び１ｃにおける曲げモーメントＭ_1a1，Ｍ_1b1及びＭ_1c1の入力に応じて、ワイヤ１ａ，１ｂ及び１ｃのそれぞれの張力を示す力ｆ_1a,ｆ_1b及びｆ_1cを演算する。具体的に、力演算部３２２１３は、（５３）式、（５４）式、（５５）式を用いて、力ｆ_1a,ｆ_1b及びｆ_1cを演算する。

伸縮量演算部３２２１４は、複数のワイヤ１ａ，１ｂ及び１ｃにおける力ｆ_1a,ｆ_1b及びｆ_1cの入力に応じて、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}を演算して、伸縮量候補値の更新を行う。具体的に、伸縮量演算部３２２１４は、（９）式と（５６）式を用いて、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}を演算して、伸縮量候補値の更新を行う。

＜３－３．シミュレーション＞
立体的に湾曲可能な湾曲可能部３１２を有する連続体ロボット３１０に対して、図１６～図１８に示す第３の実施形態における制御系を適用する。なお、湾曲可能部３１２の長さを０．０１ｍとし、長尺部１１１の長さを１ｍとする。

図１９は、本発明の第３の実施形態を示し、目標湾曲角度と湾曲角度の誤差との関係を示す特性図である。図１９では、横軸に目標湾曲角度（Reference bending angle）θ_1refをとり、縦軸に湾曲角度の誤差（Bending angle error）をとった特性図を示している。

具体的に、図１９は、目標旋回角度ζ_1refを０（ｄｅｇ）とし、目標湾曲角度θ_1refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させた場合に、湾曲可能部３１２の湾曲角度の誤差を表している。図１９において、破線は、運動学モデルを用いて演算した駆動量ｌ_k1b及びｌ_k1cを制御入力として用いた場合の比較例を示しており、この場合は、目標湾曲角度θ_1refの増減に伴って湾曲可能部３１２の角度誤差が増加してしまうことがわかる。一方、実線は、ワイヤの伸縮を考慮した駆動制御量ｌ_p1b及びｌ_p1cを制御入力として用いた場合の本実施形態を示しており、この場合は、目標湾曲角度θ_1refが変化しても、湾曲可能部３１２の角度誤差が０となることがわかる。

図２０は、本発明の第３の実施形態を示し、目標旋回角度と湾曲角度の誤差との関係を示す特性図である。図２０では、横軸に目標旋回角度（Reference rotation angle）ζ_1refをとり、縦軸に湾曲角度の誤差（Bending angle error）をとった特性図を示している。

具体的に、図２０は、目標湾曲角度θ_1refを９０（ｄｅｇ）とし、目標旋回角度ζ_1refを－１８０（ｄｅｇ）から１８０（ｄｅｇ）まで変化させた場合に、湾曲可能部３１２の湾曲角度の誤差を表している。図２０において、破線は、運動学モデルを用いて演算した駆動量ｌ_k1b及びｌ_k1cを制御入力として用いた場合の比較例を示しており、この場合は、目標旋回角度ζ_1refに依存して湾曲可能部３１２の角度誤差が生じてしまうことがわかる。一方、実線は、ワイヤの伸縮を考慮した駆動制御量ｌ_p1b及びｌ_p1cを制御入力として用いた場合の本実施形態を示しており、この場合は、目標旋回角度ζ_1refによらずに湾曲可能部３１２の角度誤差が常に０となることがわかる。以上のことから、本実施形態に係る図１６～図１８に示す制御系を適用することにより、立体的に湾曲可能な湾曲可能部３１２を有する連続体ロボット３１０の姿勢誤差を低減できることがわかる。

第３の実施形態に係る連続体ロボット制御装置３２０では、補償量演算部３２２において、湾曲可能部３１２におけるワイヤ１ｂ及び１ｃ（所定ワイヤ）ごとに、入力装置３３０から入力された湾曲可能部３１２の目標湾曲角度θ_1refと、入力装置３４０から入力された湾曲可能部３１２の目標旋回角度ζ_1refとに基づいて、それぞれ、ワイヤ１ｂ及び１ｃの補償量ｌ_e1b及びｌ_e1cを演算するようにしている。そして、加算部３２３及び位置制御部３２４において、運動学演算部３２１で算出された駆動量ｌ_k1b及びｌ_k1cのそれぞれを補償量演算部３２２で算出された補償量ｌ_e1b及びｌ_e1cで補償する処理を行って、ワイヤ１ｂ及び１ｃの駆動制御量ｌ_p1b及びｌ_p1cを設定するようにしている。
かかる構成によれば、立体的に湾曲可能な湾曲可能部３１２を有する連続体ロボット３１０においても、ワイヤの変形に起因する駆動量ｌ_k1b及びｌ_k1cの誤差を補償することができる。これにより、湾曲可能部３１２の目標姿勢と実際の姿勢との誤差を減少させることができ、その結果、体内等の経路と湾曲可能部との接触リスクの低減を実現することができる。

なお、本実施形態においても、第１の実施形態と同様に、同じ湾曲可能部に係る複数のワイヤのうちのいずれかのワイヤの伸縮量に基づいて算出した補償量を用いて、各ワイヤの駆動量を補償することができる。

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

第４の実施形態は、第２の実施形態における制御系と第３の実施形態における制御系とを組み合わせることにより、立体的に湾曲可能な複数の湾曲可能部を有する連続体ロボットを適用する形態である。具体的に、第４の実施形態における制御系では、反復計算を用いて各湾曲可能部を駆動するワイヤの伸縮量を演算する。その際、反復計算では、第２の実施形態と同様に湾曲可能部間に働く力とモーメントの連成を考慮する弾性ワイヤ駆動モデルを用いる。さらに、第４の実施形態における制御系では、第３の実施形態と同様に、目標湾曲角度と目標旋回角度から駆動量を求める。

＜４－１．モデリング＞
図２１は、本発明の第４の実施形態で用いる連続体ロボット４１０の概略構成の一例を示す図である。この図２１において、図１及び図２や図７、図１４に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。また、この図２１では、湾曲可能部４１２を挿入・抜去させる経路の一例も図示している。

湾曲可能部４１２は、図１４に示す湾曲可能部３１２を、経路に対して挿入・抜去させる方向に複数（具体的には、Ｎ個）備えて構成されている。即ち、この図２１に示す連続体ロボット４１０は、互いに直列に設けられた複数の湾曲可能部３１２－１～３１２－Ｎを有して構成されている。湾曲可能部４１２における第ｎの湾曲可能部（ｎ＝１，２，…，Ｎ）の遠位端３１２６－ｎには、ワイヤｎａ、ワイヤｎｂ及びワイヤｎｃの一端が接続されている。各ワイヤｎａ，ｎｂ及びｎｃは、第ｎ－１の湾曲可能部３１２－（ｎ－１）から第１の湾曲可能部３１２－１のワイヤガイドと長尺部１１１に案内される。また、ワイヤｎａは、固定部４１１ａに接続され、ワイヤｎｂはアクチュエータ４１１ｂに接続され、ワイヤｎｃはアクチュエータ４１１ｃに接続される。なお、図２１に示す例では、アクチュエータ４１１ｂは、ワイヤ１ｂ～ワイヤＮｂをそれぞれ独立して駆動することができる駆動機構であるものとし、また、本実施形態においては、ワイヤ１ｂ～ワイヤＮｂに対応したＮ個のアクチュエータ４１１ｂを構成する形態であってもよい。同様に、アクチュエータ４１１ｃは、ワイヤ１ｃ～ワイヤＮｃをそれぞれ独立して駆動することができる駆動機構であるものとし、また、本実施形態においては、ワイヤ１ｃ～ワイヤＮｃに対応したＮ個のアクチュエータ４１１ｃを構成する形態であってもよい。そして、本実施形態では、アクチュエータ４１１ｂ及び４１１ｃを駆動制御することにより、第ｎの湾曲可能部３１２－ｎにおける湾曲角度θ_nと旋回角度ζ_nを制御する。

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

運動学演算部４２１は、入力装置４３０及び入力装置４４０のそれぞれから、第１の湾曲可能部３１２－１から第Ｎの湾曲可能部３１２－Ｎまでの目標湾曲角度θ_nref及び目標旋回角度ζ_nref（ｎ＝１，２，…，Ｎ）の入力を受け付けて、第３の実施形態と同様に運動学モデルを用いて、各湾曲可能部の駆動量ｌ_knb及びｌ_kncを演算する。

補償量演算部４２２は、入力装置４３０及び入力装置４４０のそれぞれから、第１の湾曲可能部３１２－１から第Ｎの湾曲可能部３１２－Ｎまでの目標湾曲角度θ_nref及び目標旋回角度ζ_nrefの入力を受け付けて、各湾曲可能部の補償量ｌ_enb及びｌ_encを演算する。この際、補償量演算部４２２は、第２の実施形態と同様に湾曲可能部間の力とモーメントの連成を考慮した弾性ワイヤ駆動モデルと反復計算を用いて伸縮量Δｌ_na，Δｌ_nb及びΔｌ_ncを算出し、当該伸縮量から補償量ｌ_enb及びｌ_encを演算する。

加算部４２３は、運動学演算部４２１で算出された駆動量ｌ_knb及びｌ_kncと補償量演算部４２２で算出された補償量ｌ_enb及びｌ_encとをそれぞれ加算して、駆動制御量ｌ_pnb及びｌ_pncを演算する。即ち、加算部４２３は、運動学演算部４２１で算出された駆動量ｌ_knb及びｌ_kncのそれぞれを補償量演算部４２２で算出された補償量ｌ_enb及びｌ_encで補償する処理を行って、ワイヤｎｂの駆動制御量ｌ_pnb及びワイヤｎｃの駆動制御量ｌ_pncを算出する。また、位置制御部４２４は、連続体ロボット４１０（より具体的には、アクチュエータ４１１ｂ及び４１１ｃ）に対して駆動制御量ｌ_pnbに基づく駆動指令τ_nb及び駆動制御量ｌ_pncに基づく駆動指令τ_ncを出力する。ここで、本実施形態においては、加算部４２３及び位置制御部４２４は、ワイヤｎｂを駆動制御する駆動制御量ｌ_pnb及びワイヤｎｃを駆動制御する駆動制御量ｌ_pncを設定する設定手段を構成する。

第４の実施形態によれば、立体的に湾曲可能な複数の湾曲可能部４１２を有する連続体ロボット４１０においても、ワイヤの変形に起因する駆動量ｌ_knb及びｌ_kncの誤差を補償することができる。これにより、湾曲可能部４１２の目標姿勢と実際の姿勢との誤差を減少させることができ、その結果、体内等の経路と湾曲可能部との接触リスクの低減を実現することができる。

なお、本実施形態においても、第１の実施形態と同様に、同じ湾曲可能部に係る複数のワイヤのうちのいずれかのワイヤの伸縮量に基づいて算出した補償量を用いて、各ワイヤの駆動量を補償することができる。

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

上述した第１～第４の実施形態では、一定の目標角度が与えられるときにワイヤの伸縮を考慮する駆動制御量を求める制御系を提示するものであった。第５の実施形態は、湾曲可能部の遠位端の長手方向の移動量に依存して目標角度（例えば、目標湾曲角度）が変化する目標軌道が与えられるとき、軌道に沿うように湾曲可能部の姿勢を自動的に制御する形態である。

また、上述した第１～第４の実施形態における連続体ロボットでは、湾曲可能部の遠位端の位置はワイヤ駆動量に依存するため、目標軌道に追従するときの目標角度は、ワイヤ駆動量に依存することになる。このとき、上述した第１～第４の実施形態における制御系を用いてワイヤ駆動量を直接求めることはできない。なぜならば、上述した第１～第４の実施形態における制御系は、駆動量を求めるために目標角度を定める必要があるからである。そこで、第５の実施形態における制御系では、目標角度とワイヤ駆動量とが一定値に収束するまで更新を繰り返す反復計算により駆動制御量を求める。具体的に、第５の実施形態では、まず、目標角度の候補値を入力し、第１の実施形態における制御系を用いてワイヤ駆動量の候補値を演算する。次に、駆動量の候補値に対応する遠位端位置を演算し、遠位端位置と目標軌道から目標角度の候補値を更新する。そして、駆動量の候補値が収束するまでこの演算を反復する。

＜５－１．モデリング＞
図２３は、本発明の第５の実施形態に係る連続体ロボット制御システム５００の外観構成の一例を示す図である。この図２３に示す連続体ロボット制御システム５００は、図１に示す長尺部１１１及び湾曲可能部１１２、基台部５１１、リニアガイド５１２、及び、位置検出器５１３を有して構成されている。

第５の実施形態における連続体ロボットは、第１の実施形態と同様に、１つの湾曲可能部１１２を有して構成されている、図１に示す連続体ロボット１１０である。基台部５１１は、リニアガイド５１２に固定されており、基台部５１１を前後に押し引きすることによって、第５の実施形態における連続体ロボット１１０がリニアガイド５１２に沿って前後進するようになっている。リニアガイド５１２には、リニアガイド５１２の移動量ｚ_bを計測する位置検出器５１３が取り付けられている。このリニアガイド５１２の移動量ｚ_bは、位置検出器５１３から、後述する図２５の連続体ロボット制御装置５２０に入力される。また、リニアガイド５１２の駆動前に、上位装置（不図示）から後述する図２５の連続体ロボット制御装置５２０に対して、目標湾曲角度軌道Θ（ｚ_t）が入力される。本実施形態において、目標軌道Θ（ｚ_t）は、湾曲可能部１１２の遠位端１１２６の移動量ｚ_tの関数とする。

＜５－２．制御系＞
第５の実施形態における制御系は、遠位端１２２６の湾曲角度θ₁を目標軌道Θ（ｚ_t）に一致させる駆動制御量ｌ_p1bを求める。このとき、駆動制御量ｌ_p1bは、目標軌道Θ（ｚ_t）の関数となるため、第１の実施形態に示すように、目標湾曲角度θ_1refを定数として駆動制御量ｌ_p1bを求めることはできない。そこで、第５の実施形態では、反復計算により駆動制御量ｌ_p1bを求める。まず、湾曲可能部１１２の遠位端１１２６の移動量ｚ_tを演算する方法を示す。

図２４は、本発明の第５の実施形態を示し、図２３に示す湾曲可能部１１２における運動学モデルの一例を示す図である。この図２４に示すように、湾曲可能部１１２の遠位端１１２６と各ワイヤガイドの中心を結ぶ軸の長さをｌ_1dとすると、このｌ_1dは、駆動制御量ｌ_p1bを用いて、以下の（６１）式で表される。

したがって、湾曲可能部１１２の遠位端１１２６の移動量ｚ_tは、リニアガイド５１２の移動量ｚ_bを用いて、以下の（６２）式で表される。

次に、第５の実施形態に係る連続体ロボット制御システム５００の機能構成について説明を行う。図２５は、本発明の第５の実施形態に係る連続体ロボット制御システム５００の機能構成の一例を示す図である。この図２５に示す連続体ロボット制御システム５００は、図１に示す連続体ロボット１１０、連続体ロボット制御装置５２０、及び、位置検出器５１３を有して構成されている。

連続体ロボット制御装置５２０は、遠位端位置演算部５２１、目標角度更新部５２２、運動学演算部５２３、補償量演算部５２４、加算部５２５、収束判定部５２６、出力切換部５２７、及び、位置制御部５２８を有して構成されている。

遠位端位置演算部５２１は、リニアガイド５１２の移動量（連続体ロボット１１０の長手方向の移動量）ｚ_bとワイヤ１ｂ（所定ワイヤ）の駆動制御量における第１の駆動制御量候補値ｌ_{p1b_prev}との入力に応じて、湾曲可能部１１２の遠位端１１２６の移動量ｚ_tを演算する第３の演算手段である。具体的に、遠位端位置演算部５２１は、（６２）式を用いて湾曲可能部１１２の遠位端１１２６の移動量ｚ_tを算出する。

目標角度更新部５２２は、遠位端位置演算部５２１からの遠位端１１２６の移動量ｚ_tと上述した上位装置（不図示）からの目標湾曲角度軌道Θ（ｚ_t）との入力に応じて、移動量ｚ_tに対応する目標湾曲角度θ_1refを更新する。

運動学演算部５２３は、第１の実施形態と同様に、目標角度更新部５２２からの目標湾曲角度θ_1refの入力に応じて、ワイヤ１ｂの駆動量ｌ_k1bを演算する。また、補償量演算部５２４は、第１の実施形態と同様に、目標角度更新部５２２からの目標湾曲角度θ_1refの入力に応じて、ワイヤ１ｂの補償量ｌ_e1bを演算する。

加算部５２５は、運動学演算部５２３で算出された駆動量ｌ_k1bと、補償量演算部５２４で算出された補償量ｌ_e1bとを加算して、ワイヤ１ｂの駆動制御量における第２の駆動制御量候補値ｌ_{p1b_next}を算出する。収束判定部５２６は、第２の駆動制御量候補値ｌ_{p1b_next}が一定値に収束しているか否かを判定する。そして、収束判定部５２６は、第２の駆動制御量候補値ｌ_{p1b_next}が一定値に収束していると判定した場合、出力切換部５２７に対して、第２の駆動制御量候補値ｌ_{p1b_next}を駆動制御量ｌ_p1bとして位置制御部５２８に出力させる。また、収束判定部５２６は、第２の駆動制御量候補値ｌ_{p1b_next}が一定値に収束していないと判定した場合、出力切換部５２７に対して、第２の駆動制御量候補値ｌ_{p1b_next}を第１の駆動制御量候補値ｌ_{p1b_prev}として遠位端位置演算部５２１に出力させ、反復計算を再度行う。また、位置制御部５２８は、第１の実施形態と同様に、連続体ロボット１１０（より具体的には、アクチュエータ１１３）に対して駆動制御量ｌ_p1bに基づく駆動指令τ_1bを出力する。ここで、本実施形態においては、加算部５２５、収束判定部５２６、出力切換部５２７及び位置制御部５２８は、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1bを設定する設定手段を構成する。

なお、本実施形態では、１つの湾曲可能部１１２を有する連続体ロボット１１０に対して制御を行う制御系を示したが、例えば立体的に湾曲可能な複数の湾曲可能部４１２を有する連続体ロボット４１０に対しても同様に適用可能である。

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

第６の実施形態は、外皮構造や術具の挿通により、湾曲可能部の剛性が変化する連続体ロボットを適用する形態である。具体的に、第６の実施形態における連続体ロボットは、背景技術の欄で説明した軟性内視鏡として用いることが好適である。このような連続体ロボットでは、例えば当該連続体ロボットが体内の組織と接触しても組織を傷つけないように、連続体ロボットの表面を滑らかな外皮で覆うことが望ましい。このとき、湾曲可能部とともに外皮も湾曲駆動させる必要があるため、外皮を持たない連続体ロボットと比べて大きな張力でワイヤを駆動する必要がある。また、軟性内視鏡の中には、病変の組織を採取する際に用いる鉗子などの術具を挿通するための中空構造を備えるものがある。そして、術具を挿通した状態で連続体ロボットの湾曲可能部を駆動するためには、術具も湾曲させる必要があるため、術具を挿通していないときと比べて大きなワイヤ張力が要求される。このように、外皮や術具の有無により、湾曲可能部の剛性は変化する。この点、上述した第１～第５の実施形態の制御系では、ワイヤの伸縮量を求める際にワイヤの剛性のみを考慮するため、外皮や術具の有無により湾曲可能部の剛性が変化してしまうと角度誤差を精度良く補償できない。そこで、第６の実施形態では、湾曲可能部の剛性を変数として指定可能な制御系の実施形態を示す。

＜６－１．モデリング＞
図２６は、本発明の第６の実施形態で用いる連続体ロボット６１０の概略構成の一例を示す図である。具体的に、図２６は、第６の実施形態で用いる連続体ロボット６１０を、湾曲可能部６１２の長手方向に平行な平面で切断した断面を示す模式図である。なお、本実施形態の連続体ロボット６１０は、図１４に示す第３の実施形態の連続体ロボット３１０と同様に、立体的に湾曲可能である。なお、図２６に示す連続体ロボット６１０は、図１４に示す第３の実施形態の連続体ロボット３１０と同様に、複数のワイヤ１ａ，１ｂ及び１ｃを有しているとともに、図２６では不図示であるが、図１４に示すワイヤ１ａと接続する固定部３１１ａ、ワイヤ１ｂと接続するアクチュエータ３１１ｂ、及び、ワイヤ１ｃと接続するアクチュエータ３１１ｃを有して構成されている。

図２６（ａ）に示すように、本実施形態の連続体ロボット６１０は、長尺部６１１及び湾曲可能部６１２を含み構成されている。また、湾曲可能部６１２には、複数のワイヤガイド６１３が設けられている。さらに、長尺部６１１及び湾曲可能部６１２は、円筒状の中空部６１４を備えており、長尺部６１１の近位端から湾曲可能部６１２の遠位端まで、術具６１６を挿通することが可能となっている。また、本実施形態の連続体ロボット６１０は、湾曲可能部６１２の側面と先端及び中空部６１４を覆う外皮６１５を備えている。

外皮６１５の近位端は、長尺部６１１の遠位端に固定されているため、図２６（ｂ）に示すように、湾曲可能部６１２が湾曲駆動されると、外皮６１５の外側面は伸長し、外皮６１５の内側面は圧縮される。また、術具６１６は、湾曲可能部６１２に沿って湾曲される。そのため、上述した第１～第５の実施形態の連続体ロボットと比べて、湾曲可能部６１２の曲げ剛性が増加する。

本実施形態では、以下に示す仮定１３及び仮定１４を前提とし、外皮６１５と術具６１６の曲げ剛性を考慮する弾性ワイヤ駆動モデルを導出する。
［仮定１３］：外皮６１５と術具６１６の曲げモーメントの大きさは、湾曲可能部６１２の湾曲角度に比例する。
［仮定１４］：外皮６１５と術具６１６の曲げモーメントの回転軸は、Y₂軸と平行である。

仮定１３から、外皮６１５の曲げ剛性をｋ_mc、術具６１６の曲げ剛性をｋ_mtとすると、湾曲可能部６１２の遠位端のY₂軸まわりのモーメントのつり合いは、外皮６１５と術具６１６の曲げモーメントを表す項を（５４）式に追加した、以下の（６３）式で表される。

また、仮定１４から、Ｘ₂軸まわりのモーメントのつり合いは、外皮６１５と術具６１６の剛性の影響を受けないため、（５５）式は、本実施形態の連続体ロボット６１０においても妥当である。なお、外皮６１５の曲げ剛性ｋ_mcと術具６１６の曲げ剛性ｋ_mtは、実験により同定してもよく、また、解析的に導出してもよい。

＜６－２．制御系＞
第６の実施形態では、（６３）式を用いて、第３の実施形態と同様の反復計算を行うことで、外皮６１５と術具６１６の剛性を考慮した各ワイヤの伸縮量を導出する。

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

連続体ロボット制御装置６２０は、運動学演算部３２１、補償量演算部６２２、加算部３２３、及び、位置制御部３２４を有して構成されている。

補償量演算部６２２は、入力装置６３０から入力された湾曲可能部６１２の目標湾曲角度θ_1refと、入力装置６４０から入力された湾曲可能部６１２の目標旋回角度ζ_1refと、入力装置６３０から入力された外皮６１５の曲げ剛性ｋ_mc及び術具６１６の曲げ剛性ｋ_mtとに基づいて、ワイヤ１ｂ及び１ｃごとに補償量ｌ_e1b及びｌ_e1cを演算する。なお、本実施形態では、外皮６１５の曲げ剛性ｋ_mcと術具６１６の曲げ剛性ｋ_mtが一定であると仮定するが、剛性が湾曲角度や旋回角度に依存する非線形を有するときには、様々な湾曲角度と旋回角度に対応する剛性を施術の前に予め記憶装置に記憶しておき、術中は目標湾曲角度と目標旋回角度に応じて対応する剛性を読み出して用いてもよい。また、外皮６１５の曲げ剛性ｋ_mcと術具６１６の曲げ剛性ｋ_mtを術者が直接入力してもよい。なお、運動学演算部３２１、加算部３２３及び位置制御部３２４は、図１６に示す第３の実施形態と同様であるため、説明は省略する。

図２８は、本発明の第６の実施形態を示し、図２７に示す補償量演算部６２２の機能構成の一例を示す図である。この図２８において、図２７に示す構成及び図１７に示す第３の実施形態の構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。この図２８に示す補償量演算部６２２は、伸縮量更新部６２２１、収束判定部３２２２、出力切換部３２２３、及び、減算部３２２４を含み構成されている。

伸縮量更新部６２２１は、目標湾曲角度θ_1ref及び目標旋回角度ζ_1refと、外皮６１５の曲げ剛性ｋ_mc及び術具６１６の曲げ剛性ｋ_mtと、各ワイヤ１ａ，１ｂ及び１ｃの伸縮量における第１の伸縮量候補値Δｌ_{1a_prev}，Δｌ_{1b_prev}及びΔｌ_{1c_prev}との入力に応じて、第２の伸縮量候補値Δｌ_{1a_next}，Δｌ_{1b_next}及びΔｌ_{1c_next}に更新する。なお、収束判定部３２２２、出力切換部３２２３及び減算部３２２４は、図１７に示す第３の実施形態と同様であるため、説明は省略する。

図２９は、本発明の第６の実施形態を示し、図２８に示す伸縮量更新部６２２１の機能構成の一例を示す図である。この図２９において、図２８に示す構成及び図１８に示す第３の実施形態の構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。この図２９に示す伸縮量更新部６２２１は、ワイヤ長演算部３２２１１、モーメント演算部３２２１２、力演算部６２２１３、及び、伸縮量演算部３２２１４を含み構成されている。

力演算部６２２１３は、複数のワイヤ１ａ，１ｂ及び１ｃにおける曲げモーメントＭ_1a1，Ｍ_1b1及びＭ_1c1の入力に加えて、外皮６１５の曲げ剛性ｋ_mc及び術具６１６の曲げ剛性ｋ_mtの入力に応じて、力のつり合いの（５３）式と、モーメントのつり合いの（５５）式、（６３）式を用いて、ワイヤ１ａ，１ｂ及び１ｃのそれぞれの張力を示す力ｆ_1a,ｆ_1b及びｆ_1cを演算する。なお、ワイヤ長演算部３２２１１、モーメント演算部３２２１２及び伸縮量演算部３２２１４は、図１８に示す第３の実施形態と同様であるため、説明は省略する。

また、本実施形態では、（６３）式に示すように、外皮６１５の曲げ剛性ｋ_mcと術具６１６の曲げ剛性ｋ_mtを含むモデルを導出したが、（１０）式と（５７）式のワイヤの曲げ剛性ｋ_mを、外皮６１５や術具６１６の剛性まで考慮した剛性ｋ_meで置き換えた近似式を用いて伸縮量を演算してもよい。このとき、剛性ｋ_meは、実験により同定してもよいし、或いは、以下の（６４）式に示すように、外皮６１５の曲げ剛性ｋ_mcや術具６１６の曲げ剛性ｋ_mtをワイヤの本数ｎで除した値をワイヤの曲げ剛性ｋ_mに加算してもよい。

また、本実施形態では、連続体ロボット制御装置６２０を、立体的に湾曲可能な湾曲可能部６１２を１つ備える連続体ロボット６１０に対して適用したが、第１の実施形態のように平面上を湾曲する連続体ロボット１１０や、第４の実施形態のように複数の湾曲可能部３１２を備える連続体ロボット４１０に対しても、適用可能である。

＜６－３．シミュレーション＞
外皮６１５を備え、かつ術具６１６が挿通された連続体ロボット６１０に対して、図２７～図２９に示す第６の実施形態における制御系を適用する。なお、外皮６１５の曲げ剛性ｋ_mcと術具６１６の曲げ剛性ｋ_mtは、どちらも０．３３ Ｎｍ／ｒａｄとする。

図３０は、本発明の第６の実施形態を示し、目標旋回角度と湾曲角度の誤差との関係を示す特性図である。図３０では、図２０と同様に、横軸に目標旋回角度（Reference rotation angle）ζ_1refをとり、縦軸に湾曲角度の誤差（Bending angle error）をとった特性図を示している。

具体的に、図３０において、一番上に示された破線は、運動学モデルを用いて演算した駆動量ｌ_k1b及びｌ_k1cを制御入力として用いた場合の比較例を示しており、この場合は、目標旋回角度ζ_1refに依存して湾曲可能部６１２の大きな角度誤差が生じてしまうことがわかる。また、中央に示された点線は、第３の実施形態に係る連続体ロボット制御装置３２０を用いて演算した駆動制御量ｌ_{p1b_wo}及びｌ_{p1c_wo}を制御入力として用いた場合の比較例を示している。第３の実施形態に係る連続体ロボット制御装置３２０は、外皮６１５と術具６１６に起因する湾曲可能部６１２の剛性の増加を考慮していないため、角度誤差を完全に補償することができないことがわかる。一方、一番下に示された実線は、本実施形態に係る連続体ロボット制御装置６２０を用いて演算した駆動制御量ｌ_{p1b_w}及びｌ_{p1_wc}を制御入力として用いるときの応答を表しており、目標旋回角度ζ_1refによらずに湾曲可能部６１２の角度誤差が常に０となることがわかる。以上のことから、外皮６１５を備え、かつ術具６１６が挿通された連続体ロボット６１０に対して、図２７～図２９に示す第６の実施形態における制御系を適用することにより、連続体ロボット６１０の姿勢誤差を低減できることがわかる。

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

図１４に示す第３の実施形態の連続体ロボット３１０は、３本のワイヤ１ａ，１ｂ及び１ｃのうちの２つのワイヤ１ｂ及び１ｃにそれぞれアクチュエータ３１１ｂ及び３１１ｃを接続し、湾曲可能部３１２を立体的に湾曲させている。そのため、２つのアクチュエータ３１１ｂ及び３１１ｃのうちの一方が故障により動作しなくなると、湾曲可能部３１２は平面上を湾曲することしかできなくなってしまい、組織と連続体ロボット３１０が接触するリスクが増加する。これに対して、全てのワイヤ１ａ，１ｂ及び１ｃにアクチュエータを接続すれば、例え１つのアクチュエータが故障しても第３の実施形態と同様の制御系により、立体的な湾曲動作が可能となるため、連続体ロボット制御システムに冗長性を持たせることができる。そこで、第７の実施形態では、３本の全てのワイヤ１ａ，１ｂ及び１ｃを駆動可能な連続体ロボットに適用可能な制御系の実施形態を示す。

＜７－１．モデリング＞
本実施形態で用いる連続体ロボットは、図１４に示すワイヤ１ａが固定部３１１ａではなく、アクチュエータ３１１ａ' に接続される点が第３の実施形態に係る連続体ロボット３１０と異なる。そして、図１４に示すワイヤ１ａが固定部３１１ａではなく、アクチュエータ３１１ａ' に接続される連続体ロボットを、第７の実施形態で用いる「連続体ロボット７１０」として記載する。

この連続体ロボット７１０において、アクチュエータ３１１ａ' 、３１１ｂ及び３１１ｃを用いて、それぞれ、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃを同じ方向に同じ距離だけ駆動すると、湾曲可能部３１２に相当する湾曲可能部の湾曲角度と旋回角度とが一定のまま中心軸の長さが増減する。したがって、本実施形態の連続体ロボット７１０は、湾曲角度θ₁と旋回角度ζ₁に加えて湾曲可能部の中心軸の長さｌ_1dcの長さを制御することができる。

本実施形態で用いる連続体ロボット７１０の湾曲可能部の構成は、第３の実施形態で用いる湾曲可能部３１２と同一の構成であるため、図１５を用いて連続体ロボット７１０の運動学モデルと弾性ワイヤモデルを導出する。連続体ロボット７１０の湾曲可能部の湾曲角度がθ₁、旋回角度がζ₁のとき、Ｗ₁Ｚ₁平面における原点Ｏ₁とワイヤ１ｂとの距離Ｒ_oaは、以下の（６５）式で表される。

したがって、中心軸の長さｌ_1dcと、長さｌ_1a1との間には、以下の（６６）式が成立する。

また、（４５）式と（４６）式より、長さｌ_1b1と長さｌ_1c1は、長さｌ_1dcを用いて、それぞれ、（６７）式と（６８）式で表される。

また、ワイヤ１ａ、１ｂ及び１ｃが長手方向に剛であると仮定すると、それぞれのワイヤ長さｌ_1a1、ｌ_1b1及びｌ_1c1は、運動学モデルから導出する駆動量ｌ_k1a、ｌ_k1b及びｌ_k1cと長さｌ₁₀との和で表されるため、駆動量ｌ_k1a、ｌ_k1b及びｌ_k1cは、それぞれ、以下の（６９）式、（７０）式及び（７１）式で表される。

次に、ワイヤ１ａ、１ｂ及び１ｃが長手方向に伸縮すると仮定して、それぞれの伸縮量Δｌ_1a、Δｌ_1b及びΔｌ_1cを求める。（９）式と（５６）式より、行列Ｋ_e及びベクトルｆ，Δｌ₁をそれぞれ以下の（７２）式及び（７３）式と定義すると、Δｌ₁は、以下の（７４）式で表される。

また、（１０）式、（５３）式～（５５）式、（５７）式より、行列Ａ及びベクトルm₁をそれぞれ以下の（７５）式及び（７６）式と定義すると、以下の（７７）式が成り立つ。

したがって、（７４）式と（７７）式より、ワイヤ伸縮量Δｌ₁は、以下の（７８）式で表される。

＜７－２．制御系＞
第７の実施形態では、各アクチュエータ３１１ａ' 、３１１ｂ及び３１１ｃが自身に接続されたそれぞれのワイヤ１ａ、１ｂ及び１ｃの伸縮を補償するように、各ワイヤ１ａ、１ｂ及び１ｃの駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cを演算する。このとき、各ワイヤ１ａ、１ｂ及び１ｃの駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cは、それぞれ、以下の（７９）式～（８１）式で与えられる。

したがって、各ワイヤ１ａ、１ｂ及び１ｃの補償量ｌ_e1a、ｌ_e1b及びｌ_e1cは、それぞれ、以下の（８２）式～（８４）式で表される。

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

連続体ロボット制御装置７２０は、入力装置７３０から入力された連続体ロボット７１０の湾曲可能部の目標湾曲角度θ_1ref、入力装置７４０から入力された連続体ロボット７１０の湾曲可能部の目標旋回角度ζ_1ref、及び、入力装置７５０から入力された中心軸の長さｌ_1dcに基づいて、ワイヤ１ａを駆動制御する駆動制御量ｌ_p1a、ワイヤ１ｂを駆動制御する駆動制御量ｌ_p1b及びワイヤ１ｃを駆動制御する駆動制御量ｌ_p1cを設定し、連続体ロボット７１０（より具体的には、アクチュエータ３１１ａ' 、３１１ｂ及び３１１ｃ）に対して、駆動制御量ｌ_p1aに基づく駆動指令τ_1a、駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p1cに基づく駆動指令τ_1cを出力する。

この図３１に示す連続体ロボット制御装置７２０は、運動学演算部７２１、補償量演算部７２２、加算部７２３、及び、位置制御部７２４を有して構成されている。

運動学演算部７２１は、入力装置７３０から入力された連続体ロボット７１０の湾曲可能部の目標湾曲角度θ_1ref、入力装置７４０から入力された連続体ロボット７１０の湾曲可能部の目標旋回角度ζ_1ref、及び、入力装置７５０から入力された中心軸の長さｌ_1dcに基づいて、（６９）式～（７１）式を用いて、運動学モデルに基づく駆動量ｌ_k1a、ｌ_k1b及びｌ_k1cを演算する。

補償量演算部７２２は、各ワイヤ１ａ、１ｂ及び１ｃごとに、（７８）式を用いて伸縮量Δｌ_1a、Δｌ_1b及びΔｌ_1cを演算し、さらに、（８２）式～（８４）式を用いて補償量l_e1a、l_e1b及びl_e1cを演算する。なお、（７８）式の右辺から明らかなように、ワイヤ伸縮量Δl₁は、湾曲角度θ₁、旋回角度ζ₁、中心軸長さｌ_1dcから一意に決まる。そのため、本実施形態の補償量演算部７２２は、第１の実施形態と同様に反復計算を用いることなく伸縮量Δｌ_1a、Δｌ_1b及びΔｌ_1cを演算することができる。

加算部７２３は、各ワイヤ１ａ、１ｂ及び１ｃごとに、運動学演算部７２１で算出された駆動量ｌ_k1a、ｌ_k1b及びｌ_k1cと補償量演算部７２２で算出された補償量l_e1a、l_e1b及びl_e1cとをそれぞれ加算して、駆動制御量ｌ_p1a、ｌ_p1b、ｌ_p1cを演算する。

位置制御部７２４は、連続体ロボット７１０（より具体的には、アクチュエータ３１１ａ' 、３１１ｂ及び３１１ｃ）に対して、駆動制御量ｌ_p1aに基づく駆動指令τ_1a、駆動制御量ｌ_p1bに基づく駆動指令τ_1b及び駆動制御量ｌ_p1cに基づく駆動指令τ_1cを出力する。

なお、本実施形態では、運動学演算部７２１と補償量演算部７２２に対して入力装置７５０から入力する湾曲可能部の長さとして中心軸の長さｌ_1dcを用いるが、例えば、ワイヤ長ｌ_1a1、ｌ_1b1及びｌ_1c1のいずれかを入力してもよい。また、本実施形態では、連続体ロボット制御装置７２０を、立体的に湾曲可能な湾曲可能部を１つ備える連続体ロボット７１０に対して適用したが、第１の実施形態のように平面上を湾曲する連続体ロボット１１０や、第４の実施形態のように複数の湾曲可能部３１２を備える連続体ロボット４１０に対しても、適用可能である。

＜７－３．シミュレーション＞
３本の全てのワイヤ１ａ、１ｂ及び１ｃを駆動可能な連続体ロボット７１０に対して、第７の実施形態における制御系を適用する。なお、本実施形態では、長さｌ_1dcと長さｌ₁₀をどちらも１０ｍｍとする。

図３２は、本発明の第７の実施形態を示し、目標湾曲角度と駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cとの関係を示す特性図である。図３２では、横軸に目標湾曲角度（Reference bending angle）θ_1refをとり、縦軸に駆動制御量（Wire driving distance）をとった特性図を示している。

具体的に、図３２では、目標旋回角度ζ_1refを０（ｄｅｇ）とし、目標湾曲角度θ_1refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させた場合の駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cを、それぞれ実線、点線及び破線を用いて表している。第７の実施形態に係る連続体ロボット制御装置７２０は、３本のワイヤ１ａ、１ｂ及び１ｃを駆動するため、図３２に示すように、目標湾曲角度が変化するとワイヤ１aを含む全てのワイヤ１ａ、１ｂ及び１ｃの駆動制御量が増減する。また、本実施形態に係る連続体ロボット制御装置７２０は、中心軸の長さが常にｌ_1dcとなるように、駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cを演算するため、目標湾曲角度θ_1refが正の場合には、中心軸に対して内側に位置するワイヤ１aの駆動方向はワイヤを引く方向（負の方向）となり、また、中心軸に対して外側に位置するワイヤ１ｂ及び１ｃの駆動方向はワイヤを押す方向（正の方向）となることがわかる。

図３３は、本発明の第７の実施形態を示し、目標湾曲角度と湾曲角度の誤差との関係を示す特性図である。図３３では、横軸に目標湾曲角度（Reference bending angle）θ_1refをとり、縦軸に湾曲角度の誤差をとった特性図を示している。

具体的に、図３３では、目標旋回角度ζ_1refを０（ｄｅｇ）とし、目標湾曲角度θ_1refを－９０（ｄｅｇ）から９０（ｄｅｇ）まで変化させた場合の湾曲角度の誤差を表している。図３３において、破線は、運動学モデルを用いて演算した駆動量ｌ_k1a、ｌ_k1b及びｌ_k1cを制御入力として用いた場合の比較例を示しており、この場合は大きな角度誤差が生じてしまうことがわかる。また、実線は、第７の実施形態の連続体ロボット制御装置７２０を用いて演算した駆動制御量ｌ_p1a、ｌ_p1b及びｌ_p1cを制御入力として用いた場合の角度誤差を表しており、連続体ロボット７１０の湾曲可能部の角度誤差が常に０となることがわかる。以上のことから、３本の全てのワイヤ１ａ、１ｂ及び１ｃを駆動可能な連続体ロボット７１０に対して、図３１に示す第７の実施形態における制御系を適用することにより、連続体ロボット７１０の姿勢誤差を低減できることがわかる。

（その他の実施形態）
上述した第１～第７の実施形態では、ワイヤとワイヤガイド、及びワイヤと長尺部の摩擦を考慮していないが、摩擦力が既知のときにはこれを考慮して伸縮量を演算してもよい。具体的には、（９）式の左辺に摩擦力ｆ_r1a及びｆ_r1bを加えた、以下の（８５）式を、（９）式の代わりに用いて、伸縮量を演算する形態をとり得る。

なお、本実施形態においても、第１の実施形態と同様に、同じ湾曲可能部に係る複数のワイヤのうちのいずれかのワイヤの伸縮量に基づいて算出した補償量を用いて、各ワイヤの駆動量を補償することができる。

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

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

１００：連続体ロボット制御システム、１１０：連続体ロボット、１２０：連続体ロボット制御装置、１２１：運動学演算部、１２２：補償量演算部、１２３：加算部、１２４：位置制御部、１３０：入力装置、θ_1ref：湾曲可能部の目標湾曲角度、ｌ_k1b：ワイヤ１ｂの駆動量、ｌ_e1b：ワイヤ１ｂの補償量、ｌ_p1b：ワイヤ１ｂの駆動制御量、τ_1b：駆動制御量ｌ_p1bに基づく駆動指令

Claims (10)

1. 複数のワイヤのうちの少なくとも一部のワイヤを駆動することにより湾曲する湾曲可能部を有する連続体ロボットの動作を制御する連続体ロボット制御装置であって、
   前記湾曲可能部の湾曲角度の目標値である目標湾曲角度および前記湾曲可能部の旋回角度の目標値である目標旋回角度に基づいて、前記少なくとも一部のワイヤの駆動量を演算する第１の演算手段と、
   前記目標湾曲角度と、前記目標旋回角度と、前記目標湾曲角度および前記目標旋回角度における前記複数のワイヤのうちのいずれかのワイヤの伸縮量とに基づいて、前記駆動量を補償する補償量を演算する第２の演算手段と、
   前記第１の演算手段で算出された駆動量と、前記第２の演算手段で算出された補償量とに基づいて、前記少なくとも一部のワイヤを駆動制御する駆動制御量を設定する設定手段と、
   を有することを特徴とする連続体ロボット制御装置。
2. 前記連続体ロボットは、互いに直列に設けられた複数の前記湾曲可能部を有して構成されており、
   前記第１の演算手段は、前記複数の湾曲可能部における湾曲可能部ごとに、前記目標湾曲角度に基づいて前記駆動量を演算し、
   前記第２の演算手段は、前記複数の湾曲可能部における湾曲可能部ごとに、前記補償量を演算し、
   前記設定手段は、前記複数の湾曲可能部における湾曲可能部ごとに、前記第１の演算手段で算出された駆動量と前記第２の演算手段で算出された補償量とに基づいて前記駆動制御量を設定することを特徴とする請求項１に記載の連続体ロボット制御装置。
3. 前記第２の演算手段は、前記目標湾曲角度の入力に応じて前記複数のワイヤのうちの一のワイヤの伸縮量を算出し、当該算出した伸縮量に基づいて他の一の前記ワイヤの補償量を演算することを特徴とする請求項１または２に記載の連続体ロボット制御装置。
4. 前記第２の演算手段は、
   前記目標湾曲角度と、一の前記ワイヤの伸縮量における第１の伸縮量候補値との入力に応じて、他の一のワイヤにおける第２の伸縮量候補値に更新する伸縮量更新部と、
   前記第２の伸縮量候補値を前記他の一のワイヤの伸縮量として用いて前記他の一のワイヤに対する補償量を算出する補償量算出部と、
   前記第２の伸縮量候補値が一定値に収束している場合に当該第２の伸縮量候補値を前記他の一のワイヤの伸縮量として前記補償量算出部に出力し、前記第２の伸縮量候補値が前記一定値に収束していない場合に当該第２の伸縮量候補値を前記第１の伸縮量候補値として前記伸縮量更新部に出力する出力処理部と、
   を含み構成されていることを特徴とする請求項１または２に記載の連続体ロボット制御装置。
5. 前記伸縮量更新部は、
   前記目標湾曲角度と前記第１の伸縮量候補値との入力に応じて、前記湾曲可能部における前記複数のワイヤごとにワイヤ長を演算するワイヤ長演算部と、
   前記目標湾曲角度と前記複数のワイヤにおける前記ワイヤ長との入力に応じて、前記複数のワイヤごとに曲げモーメントを演算するモーメント演算部と、
   前記複数のワイヤにおける前記曲げモーメントの入力に応じて、前記複数のワイヤの張力を演算する力演算部と、
   前記張力の入力に応じて、前記第２の伸縮量候補値を演算する伸縮量演算部と、
   を含み構成されていることを特徴とする請求項４に記載の連続体ロボット制御装置。
6. 前記伸縮量更新部は、前記第１の伸縮量候補値として、前記他の一のワイヤを含む前記複数のワイヤの伸縮量における伸縮量候補値を更新するものであり、
   前記伸縮量更新部は、
   前記目標湾曲角度と前記第１の伸縮量候補値との入力に応じて、前記湾曲可能部における前記複数のワイヤの各々についてワイヤ長を演算するワイヤ長演算部と、
   前記目標湾曲角度と前記湾曲可能部の旋回角度の目標値である目標旋回角度と前記複数のワイヤにおける前記ワイヤ長との入力に応じて、前記複数のワイヤの各々について曲げモーメントを演算するモーメント演算部と、
   前記複数のワイヤにおける前記曲げモーメントの入力に応じて、前記複数のワイヤの張力を演算する力演算部と、
   前記複数のワイヤにおける前記張力の入力に応じて、前記他のワイヤを含む前記複数のワイヤの伸縮量における前記第２の伸縮量候補値を演算する伸縮量演算部と、
   を含み構成されていることを特徴とする請求項４に記載の連続体ロボット制御装置。
7. 前記連続体ロボットの長手方向の移動量と前記ワイヤの駆動制御量における第１の駆動制御量候補値との入力に応じて、前記湾曲可能部の遠位端の移動量を演算する第３の演算手段と、
   前記湾曲可能部の遠位端の移動量の入力に応じて、前記目標湾曲角度を更新する目標角度更新手段と、
   を更に有し、
   前記設定手段は、前記第１の演算手段が前記目標角度更新手段で更新した前記目標湾曲角度に基づき算出した駆動量と、前記第２の演算手段が前記目標角度更新手段で更新した前記目標湾曲角度に基づき算出した補償量とに基づいて、前記ワイヤの駆動制御量における第２の駆動制御量候補値を演算し、前記第２の駆動制御量候補値が一定値に収束している場合に当該第２の駆動制御量候補値を前記駆動制御量として設定し、前記第２の駆動制御量候補値が前記一定値に収束していない場合には前記第２の駆動制御量候補値を前記第１の駆動制御量候補値として前記第３の演算手段に出力することを特徴とする請求項１乃至６のいずれか１項に記載の連続体ロボット制御装置。
8. 前記ワイヤの長さは、前記湾曲可能部の長さの１０倍以上であることを特徴とする請求項１乃至７のいずれか１項に記載の連続体ロボット制御装置。
9. 複数のワイヤのうちの少なくとも一部のワイヤを駆動することにより湾曲する湾曲可能部を有する連続体ロボットの動作を制御する連続体ロボット制御方法であって、
   前記湾曲可能部の湾曲角度の目標値である目標湾曲角度および前記湾曲可能部の旋回角度の目標値である目標旋回角度に基づいて、前記少なくとも一部のワイヤの駆動量を演算する第１の演算ステップと、
   前記目標湾曲角度と、前記目標旋回角度と、前記目標湾曲角度および前記目標旋回角度における前記複数のワイヤのうちのいずれかのワイヤの伸縮量とに基づいて、前記駆動量を補償する補償量を演算する第２の演算ステップと、
   前記第１の演算ステップで算出された駆動量と、前記第２の演算ステップで算出された補償量とに基づいて、前記少なくとも一部のワイヤを駆動制御する駆動制御量を設定する設定ステップと、
   を有することを特徴とする連続体ロボット制御方法。
10. 請求項１乃至８のいずれか１項に記載の連続体ロボット制御装置における各手段としてコンピュータを機能させるためのプログラム。

Images