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

Description

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

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

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

このような連続体ロボットでは、湾曲可能部を湾曲させるための駆動力伝達機構としてワイヤ等を用いることで、ワイヤ等を駆動するアクチュエータ等の駆動部を基台に設置し、湾曲可能部を細径化する方式がよく用いられる。これにより、体内の深部に到達することが可能となるが、連続体ロボットの湾曲可能部を細径化すると剛性が低下する。そのため、連続体ロボットの湾曲可能部が体腔などの内壁に当接すると、連続体ロボット（湾曲可能部）がその中心軸周りにねじれることが起こりうる。そして、このねじれの発生により、ワイヤの駆動量と連続体ロボット（湾曲可能部）の姿勢との対応関係が設計値からずれるため、連続体ロボット（湾曲可能部）の制御の精度が低下する恐れがある。この点に関して、例えば、特許文献１には、連続体ロボットの湾曲可能部がねじれた場合にも、ねじれ量に基づいてワイヤの駆動変位量を制御することで湾曲可能部の姿勢を目標姿勢に一致させる技術が記載されている。

特開２０１９－１２２４９１号公報

特許文献１では、湾曲可能部のねじれによる制御誤差を補償するために、当該湾曲可能部において基台側に最も近い位置（近位端）を通過するワイヤの位相角にねじれ量を踏まえた値を新たな位相角と仮定して、湾曲可能部を通過するワイヤの長さを演算する。この特許文献１に記載の技術では、ワイヤは、湾曲可能部の近位端から、当該湾曲可能部において基台側から最も遠い位置（遠位端）のワイヤガイドまで、直線状に進むと仮定している。しかしながら、実際の連続体ロボットでは、湾曲可能部にねじれが生じると、ワイヤは、連続体ロボット（湾曲可能部）の中心軸に沿って、らせん状に進む。したがって、特許文献１に記載の技術では、連続体ロボットの湾曲可能部のねじれに起因する当該湾曲可能部におけるワイヤの長さ（当該湾曲可能部を通過するワイヤの長さ）を精度良く算出することができず、その結果、連続体ロボットの制御性能を向上させることが困難であるという問題があった。

本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの湾曲可能部にねじれが生じた場合にも、連続体ロボットの制御性能の向上を実現できる仕組みを提供することを目的とする。

本発明の連続体ロボットの制御システムは、ワイヤが駆動することによって湾曲する湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、前記湾曲可能部のねじれ角度を取得するねじれ角度取得手段と、前記ねじれ角度取得手段によって取得されたねじれ角度に基づいて、前記湾曲可能部における前記ワイヤの長さを演算する運動学演算手段と、を有し、前記運動学演算手段は、前記湾曲可能部を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度、旋回角度およびねじれ角度に基づいて、当該微小区間における前記ワイヤの長さを演算するワイヤ長演算手段と、前記ワイヤ長演算手段で得られた前記複数の微小区間における前記ワイヤの長さを加算することによって、前記湾曲可能部における前記ワイヤの長さを算出する加算手段と、を有する。
また、本発明は、上述した連続体ロボットの作動方法、及び、上述した連続体ロボットの制御システムとしてコンピュータを機能させるためのプログラムを含む。

本発明によれば、連続体ロボットの湾曲可能部にねじれが生じた場合にも、当該湾曲可能部におけるワイヤの長さを精度良く算出することができるため、連続体ロボットの制御性能の向上を実現することができる。

本発明の第１の実施形態で用いる連続体ロボットの外観構成の一例を示す図である。 図１に示す湾曲可能部を拡大した概略構成図である。 図１に示す湾曲可能部のモデルの一例を示す図である。 図３に示すｊ番目の微小区間のモデルの一例を示す図である。 本発明の第１の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第１の実施形態を示し、図５に示す運動学演算部の機能構成の一例を示す図である。 本発明の第１の実施形態に係る連続体ロボット制御システムを用いたシミュレーションの結果を示す図である。 本発明の第１の実施形態に係る連続体ロボット制御システムを用いたシミュレーションの結果を示す図である。 本発明の第２の実施形態で用いる連続体ロボットの外観構成の一例を示す図である。 図９に示す第１の湾曲可能部及び第２の湾曲可能部のモデルの一例を示す図である。 本発明の第２の実施形態に係る連続体ロボット制御システムの機能構成の一例を示す図である。 本発明の第３の実施形態に係る連続体ロボット制御システムの外観構成の一例を示す図である。 本発明の第３の実施形態を示し、図５に示す運動学演算部の機能構成の一例を示す図である。

以下に、図面を参照しながら、本発明を実施するための形態（実施形態）について説明する。なお、以下に記載する本発明の実施形態では、湾曲可能部を湾曲させるための駆動力伝達機構としてワイヤを用いた連続体ロボットを想定した例を説明し、当該連続体ロボットに対して本発明に係る制御システム及び制御方法を適用する。そして、以下に記載する本発明の実施形態では、ワイヤが連続体ロボット（湾曲可能部）の中心軸に沿ってらせん状に進むと仮定して湾曲可能部におけるワイヤの長さを算出することで、連続体ロボットの制御性能の向上を図る。

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

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

湾曲可能部１１０は、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃのうちの少なくとも１つのワイヤが駆動することによって立体的に湾曲可能な構成部である。ここで、本実施形態においては、立体的に湾曲可能な湾曲可能部１１０を１つ備える連続体ロボット１００－１を想定した形態となっている。湾曲可能部１１０の遠位端には、湾曲可能部１１０の中心軸周りのねじれ量を示すねじれ角度τ₁を検出するための小型のセンサコイル１１１１が設けられている。

長尺部１２０は、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃを内包し、例えば外力に対して受動的に湾曲する構成部である。ここで、図１に示す例では、長尺部１２０の先端面１２０１を近位端とし、当該近位端からからセンサコイル１１１１が設けられている遠位端までが、湾曲可能部１１０であるものとする。

アクチュエータ１３０－１ａ～１３０－１ｃは、ワイヤ１ａ～１ｃを駆動する駆動部である。具体的に、アクチュエータ１３０－１ａはワイヤ１ａを駆動し、アクチュエータ１３０－１ｂはワイヤ１ｂを駆動し、アクチュエータ１３０－１ｃはワイヤ１ｃを駆動する。

図２は、図１に示す湾曲可能部１１０を拡大した概略構成図である。この図２において、図１に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃは、図２に示す複数のワイヤガイド１１２と図１に示す長尺部１２０に設けられた孔に案内される。そして、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃは、一端が図２に示す湾曲可能部１１０の遠位端１１１（センサコイル１１１１が設けられているワイヤガイド）に接続され、他端が図１に示す対応するアクチュエータ１３０－１ａ～１３０－１ｃに接続される。本実施形態では、アクチュエータ１３０－１ａ～１３０－１ｃを湾曲可能部１１０の中心軸方向に駆動すると、ワイヤ１ａ～１ｃがそれぞれ押し引きされ、湾曲可能部１１０が立体的に湾曲する。

図３は、図１に示す湾曲可能部１１０のモデルの一例を示す図である。具体的に、図３は、ワイヤ１ａ～１ｃが、連続体ロボット１００－１の湾曲可能部１１０の中心軸１１３に沿って、らせん状に進むモデルを導出するための図である。本実施形態では、この図３に示すモデルを用いて、連続体ロボット１００－１の湾曲可能部１１０におけるねじれを考慮し、湾曲可能部１１０の姿勢を目標姿勢と一致させる制御入力を与える連続体ロボット制御装置を設計する。

以下に、本実施形態で用いる記号の定義を示す。
θ₁：第１の湾曲可能部１１０の絶対湾曲角度
ζ₁：第１の湾曲可能部１１０の絶対旋回角度
τ₁：第１の湾曲可能部１１０の絶対ねじれ角度
ｌ_1a1：第１の湾曲可能部１１０におけるワイヤ１ａの長さ
ｌ_1b1：第１の湾曲可能部１１０におけるワイヤ１ｂの長さ
ｌ_1c1：第１の湾曲可能部１１０におけるワイヤ１ｃの長さ
ｒ_g：湾曲可能部１１０の中心軸１１３から、ワイヤ１ａ，ワイヤ１ｂ，ワイヤ１ｃまでの距離
ｌ₁₀：湾曲角度が０（ｄｅｇ）のときの第１の湾曲可能部１１０のワイヤの長さ
Δθ_1,j：第１の湾曲可能部１１０におけるｊ番目の微小区間の湾曲角度
Δｌ_1,j：第１の湾曲可能部１１０におけるｊ番目の微小区間の長さ

図３では、太い実線で、湾曲可能部１１０におけるワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃを示し、一点鎖線で、湾曲可能部１１０の中心軸１１３を示している。また、図３において、細い実線は、湾曲可能部１１０を長手方向における中心軸１１３の方向に、複数のｍ₁個の微小区間に仮想的に分割した断面を示している。

図３に示す湾曲可能部１１０のモデルでは、図１に示す湾曲可能部１１０の近位端に相当する長尺部１２０の先端面１２０１の中心を原点Ｏ₁としている。そして、図３に示す湾曲可能部１１０のモデルでは、先端面１２０１における中心軸１１３の方向にＺ₁軸をとり、原点Ｏ₁から先端面１２０１におけるワイヤ１ａの位置に向かう方向にＸ₁軸をとり、Ｘ₁軸とＺ₁軸に直交する方向にＹ₁軸をとっている。また、図３に示す湾曲可能部１１０のモデルでは、湾曲可能部１１０の遠位端１１１における中心軸１１３の方向にＺ₂軸をとり、Ｚ₁軸とＺ₂軸とのなす角度を湾曲可能部１１０の湾曲角度θ₁としている。さらに、図３に示す湾曲可能部１１０のモデルでは、Ｘ₁Ｙ₁平面内で原点Ｏ₁から遠位端１１１の中心に向かう方向にＷ₁軸をとり、Ｘ₁軸とＷ₁軸とのなす角度を湾曲可能部１１０の旋回角度ζ₁としている。

連続体ロボット１００－１の湾曲可能部１１０にねじれが生じていないとき、ワイヤ１は、Ｗ₁Ｚ₁平面と平行な平面上を進むため、ワイヤ１を連続体のまま扱っても、ワイヤ１の長さを容易に演算することができる。

一方、連続体ロボット１００－１の湾曲可能部１１０にねじれが生じると、図３に示すように、ワイヤ１ａ～１ｃは、湾曲可能部１１０の長手方向における中心軸１１３に沿ってらせん状に進む。そのため、ワイヤ１ａ～１ｃを連続体としたまま、当該ワイヤの長さを算出しようとすると複雑な演算が必要となり、連続体ロボット制御装置の計算負荷が増加してしまう。そこで、本実施形態では、図３に示すように、湾曲可能部１１０を、長手方向（中心軸１１３の方向）に複数の微小区間に仮想的に分割して離散化し、分割によって得られた微小区間内では、ワイヤ１ａ～１ｃは直線状であると近似して当該ワイヤの長さを算出する。図３には、先端面１２０１から数えて、ｊ番目（ｊ＝１，２，…，ｍ₁）の微小区間における座標系を示している。このｊ番目の微小区間における座標系は、先端面１２０１の側の中心Ｏ_1,jを原点として、遠位端１１１の方向にＺ_1,j軸をとり、Ｚ_1,j軸と直交し且つＸ₁軸と同一平面上にＸ_1,j軸をとり、Ｘ_1,j軸とＺ_1,j軸との両軸と直交する方向にＹ_1,j軸をとっている。また、Ｘ_1,jＹ_1,j平面とＷ₁Ｚ₁平面との交線上にＷ_1,j軸をとっている。

図４は、図３に示すｊ番目の微小区間のモデルの一例を示す図である。
図４では、Ｗ_1,j軸とＷ_1,j+1軸との交点を点Ｔ_1,jとしている。このとき、点Ｏ_1,jと点Ｔ_1,jとを結ぶベクトルと、点Ｏ_1,j+1と点Ｔ_1,jとを結ぶベクトルとのなす角度を、図４に示すように微小区間の湾曲角度Δθ_1,jとする。また、図４では、ｊ番目の微小区間における先端面１２０１の側の面とワイヤ１ａとの交点を点Ｐ_1a1,jとし、原点Ｏ_1,jと点Ｐ_1a1,jを結ぶベクトルとＸ_1,j軸とのなす角度をワイヤのねじれ角度（「ねじれ角度」と称してもよい）τ_1,jとしている。さらに、図４では、ｊ番目の微小区間における遠位端１１１の側の面とワイヤ１ａとの交点を点Ｐ_1a1,j+1とし、点Ｐ_1a1,j+1におけるＸ_1,jＹ_1,j平面上への射影点を点Ｐ'_1a1,jとしている。この場合、原点Ｏ_1,jと点Ｐ'_1a1,jとを結ぶベクトルとＸ_1,j軸とのなす角度は、ｊ＋１番目の微小区間のねじれ角度τ_1,j+1となる。

そして、本実施形態では、以下に記載の仮定をおき、連続体ロボット１００－１の湾曲可能部１１０におけるモデルを導出する。
仮定１．微小区間内でワイヤは直線状となる。
仮定２．湾曲可能部１１０は、曲率一定に変形する。
仮定３．ワイヤガイドとワイヤとの間に働く摩擦力を考慮せず、ねじれ角度は、ワイヤガイドの影響を受けることなく一定の割合で連続的に変化する。
仮定４．ワイヤの伸縮を考慮しない。

まず、湾曲可能部１１０のｊ番目の微小区間を通過するワイヤ１ａの長さｌ_1a1,jを導出する。仮定１より、ワイヤ１ａの長さｌ_1a1,jは、図４に示す線分Ｐ_1a1,j‐Ｐ_1a1,j+1の長さとなる。そして、原点Ｏ_1,jから点Ｐ_1a1,jへのベクトルｐ_1a1,jは、以下の（１）式で表される。

また、原点Ｏ_1,jから点Ｐ'_1a1,jへのベクトルｐ'_1a1,jは、以下の（２）式で表される。

また、原点Ｏ_1,jから点Ｔ_1,jへのベクトルｔ_1,jは、以下の（３）式で表される。

また、湾曲可能部１１０を、各微小区間の長さが等しくなるように分割すると、ｊ番目の微小区間の長さΔｌ_1,jは、以下の（４）式で表される。

このとき、仮定２と仮定３より、ｊ番目の微小区間の湾曲角度Δθ_1,j、旋回角度ζ_1,j及びねじれ角度τ_1,jは、それぞれ、湾曲可能部１１０の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁を用いて、以下の（５）式～（７）式で表される。

なお、点Ｐ_1a1,j+1は、点Ｔ_1,jを通りＷ_1,jＺ_1,j平面と直交する軸を中心として、点Ｐ'_1a1,jを湾曲角度Δθ_1,jだけ回転させた点である。したがって、原点を中心として、Ｚ_1,j軸周りに旋回角度ζ_1,jだけ回転させる回転行列をＲｚ（ζ_1,j）とし、Ｙ_1,j軸周りに湾曲角度Δθ_1,jだけ回転させる回転行列をＲｙ（Δθ_1,j）とすると、原点Ｏ_1,jから点Ｐ_1a1,j+1へのベクトルｐ_1a1,j+1は、ベクトルｐ'_1a1,jとベクトルｔ_1,jを用いて、以下の（８）式で表される。

ただし、行列Ｒｚ（ζ_1,j）及び行列Ｒｙ（Δθ_1,j）は、それぞれ、以下の（９）式及び（１０）式とする。

上述したように、ｊ番目の微小区間を通過するワイヤ１ａの長さｌ_1a1,jは、線分Ｐ_1a1,j‐Ｐ_1a1,j+1の長さとなるため、このワイヤ１ａの長さｌ_1a1,jは、以下の（１１）式で表される。

また、仮定４より、湾曲可能部１１０におけるワイヤ１ａの長さ（湾曲可能部１１０を通過するワイヤ１ａの全長）ｌ_1a1は、湾曲可能部１１０の各微小区間におけるワイヤ１ａの長さの和となるため、湾曲可能部１１０におけるワイヤ１ａの長さｌ_1a1は、以下の（１２）式で表される。

続いて、上述したワイヤ１ａと同様に、ワイヤ１ｂ及びワイヤ１ｃについても説明する。ｊ番目の微小区間における先端面１２０１の側の面とワイヤ１ｂ及びワイヤ１ｃとの交点を、それぞれ、点Ｐ_1b1,j及び点Ｐ_1c1,jとすると、原点Ｏ_1,jから点Ｐ_1b1,j及び点Ｐ_1c1,jへのベクトルｐ_1b1,j及びベクトルｐ_1c1,jは、それぞれ、以下の（１３）式及び（１４）式で表される。

また、原点Ｏ_1,jから射影点Ｐ'_1b1,j及び射影点Ｐ'_1c1,jへのベクトルｐ'_1b1,j及びベクトルｐ'_1c1,jは、それぞれ、以下の（１５）式及び（１６）式で表される。

そして、原点Ｏ_1,jから、ｊ番目の微小区間における遠位端１１１の側の面とワイヤ１ｂ及びワイヤ１ｃとの交点Ｐ_1b1,j+1及びＰ_1c1,j+1へのベクトルｐ_1b1,j+1及びベクトルｐ_1c1,j+1は、それぞれ、以下の（１７）式及び（１８）式で表される。

よって、湾曲可能部１１０におけるワイヤ１ｂの長さｌ_1b1及びワイヤ１ｃの長さｌ_1c1は、それぞれ、以下の（１９）式及び（２０）式で表される。

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

連続体ロボット制御システム１０－１は、図５に示すように、連続体ロボット１００（より具体的には、図１に示す連続体ロボット１００－１）、連続体ロボット制御装置２００、及び、入力装置３１０を有して構成されている。即ち、連続体ロボット制御システム１０－１は、ワイヤ１ａ～１ｃが駆動することによって湾曲する湾曲可能部１１０と、ワイヤ１ａ～１ｃを駆動するアクチュエータ（駆動部）１３０－１ａ～１３０－１ｃを備える連続体ロボット１００－１の制御システムである。

入力装置３１０は、連続体ロボット制御装置２００に対して情報を入力する装置であり、入力部３１１及び磁気センサ部３１２を有して構成されている。入力部３１１は、連続体ロボット制御装置２００に対して、湾曲可能部１１０の湾曲角度（目標湾曲角度）θ₁及び湾曲可能部１１０の旋回角度（目標旋回角度）ζ₁を入力する構成部である。磁気センサ部３１２は、湾曲可能部１１０の遠位端１１１に設けられているセンサコイル１１１１で得られた磁界の強さ・向きに基づいて、湾曲可能部１１０の中心軸１１３周りのねじれ角度τ₁を計測し、これを湾曲可能部１１０のねじれ角度τ₁として連続体ロボット制御装置２００に入力する構成部である。ここで、本実施形態においては、磁気センサ部３１２及びセンサコイル１１１１は、湾曲可能部１１０のねじれ角度τ₁を取得する「ねじれ角度取得手段」を構成する。

連続体ロボット制御装置２００は、入力装置３１０から入力された情報に基づいて、連続体ロボット１００（より具体的には、図１に示す連続体ロボット１００－１）を制御する装置であり、運動学演算部２１１及び位置制御部２２１を有して構成されている。運動学演算部２１１は、入力装置３１０から入力された情報である湾曲可能部１１０の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁に基づいて、湾曲可能部１１０におけるワイヤ１ａ，１ｂ及び１ｃの長さｌ_1a1，ｌ_1b1及びｌ_1c1を演算する構成部である。位置制御部２２１は、湾曲可能部１１０におけるワイヤ１ａ，１ｂ及び１ｃの長さが、それぞれ、長さｌ_1a1，ｌ_1b1及びｌ_1c1となるように、連続体ロボット１００－１のアクチュエータ１３０－１ａ，１３０－１ｂ及び１３０－１ｃに対して駆動指令ｆ_1a，ｆ_1b及びｆ_1cを出力する構成部である。即ち、位置制御部２２１は、運動学演算部２１１で得られた湾曲可能部１１０におけるワイヤ１ａ～１ｃの長さｌ_1a1～ｌ_1c1に基づいて、アクチュエータ１３０－１ａ～１３０－１ｃによるワイヤ１ａ～１ｃの駆動を制御する制御手段である。

図６は、本発明の第１の実施形態を示し、図５に示す運動学演算部２１１の機能構成の一例を示す図である。この図６において、図５に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。なお、図６では、湾曲可能部１１０を湾曲させるためのワイヤ１ａ～１ｃのうち、ワイヤ１ａのみを対象としてその長さｌ_1a1を算出する構成例のみを図示しているが、ワイヤ１ｂ及び１ｃについても、ワイヤ１ａの場合と同様の構成部を備えているものとする。

運動学演算部２１１は、図６に示すように、角度演算部６１０、ワイヤ長演算部６２０、及び、加算部６３０を有して構成されている。

角度演算部６１０は、入力装置３１０から入力された情報である湾曲可能部１１０の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁に基づいて、図３を用いて説明した、湾曲可能部１１０を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度Δθ、旋回角度ζ及びねじれ角度τを演算する角度演算手段である。具体的に、図６では、角度演算部６１０によって算出された、ｊ－１番目の微小区間、ｊ番目の微小区間及びｊ＋１番目の微小区間における湾曲角度Δθ、旋回角度ζ及びねじれ角度τ（当該微小区間の一端のねじれ角度及び他端のねじれ角度）を図示している。本実施形態においては、角度演算部６１０は、（５）式～（７）式を用いて、各微小区間の湾曲角度Δθ_1,j、旋回角度ζ_1,j及びねじれ角度τ_1,jを演算する。

ワイヤ長演算部６２０は、湾曲可能部１１０を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度Δθ、旋回角度ζ及びねじれ角度τに基づいて、当該微小区間におけるワイヤ１ａの長さｌ_1a1,j-1、ｌ_1a1,j、ｌ_1a1,j+1、・・・を演算するワイヤ長演算手段である。具体的に、ワイヤ長演算部６２０は、角度演算部６１０で得られた、複数の微小区間におけるそれぞれの微小区間ごとの湾曲角度Δθ、旋回角度ζ及びねじれ角度τを用いて、当該微小区間におけるワイヤ１ａの長さｌ_1a1,j-1、ｌ_1a1,j、ｌ_1a1,j+1、・・・を演算する。この際、図６に示すワイヤ長演算部６２０では、複数の微小区間におけるそれぞれの微小区間に対応して、複数のワイヤ長演算器６２１が構成されている。本実施形態においては、ワイヤ長演算部６２０は、（１）式～（４）式と、（８）式～（１１）式を用いて、ｊ番目の微小区間におけるワイヤ１ａの長さｌ_1a1,jを演算する。

加算部６３０は、ワイヤ長演算部６２０で得られた複数の微小区間におけるワイヤ１ａの長さｌ_1a1,j-1、ｌ_1a1,j、ｌ_1a1,j+1、・・・を加算することによって、湾曲可能部１１０におけるワイヤ１ａの長さｌ_1a1を算出する加算手段である。本実施形態においては、加算部６３０は、（１２）式を用いて、湾曲可能部１１０におけるワイヤ１ａの長さ（湾曲可能部１１０を通過するワイヤ１ａの長さ）ｌ_1a1を算出する。

＜１－３．シミュレーション＞
上記＜１－１＞で導出した連続体ロボット１００－１のモデルと、上記＜１－２＞で提示した制御系を用いてシミュレーションを行う。本実施形態では、微小区間の数ｍ₁を２０とし、湾曲可能部１１０の長さｌ₁₀を０．０３ｍとし、湾曲可能部１１０の中心軸１１３から、ワイヤ１ａ，ワイヤ１ｂ，ワイヤ１ｃまでの距離ｒ_gを０．００２ｍとする。

まず、本実施形態の制御系（連続体ロボット制御システム１０－１）を適用することで、連続体ロボット１００－１の湾曲可能部１１０がねじれる場合においても、湾曲可能部１１０におけるワイヤの長さを精度良く算出可能なことを説明する。

図７は、本発明の第１の実施形態に係る連続体ロボット制御システム１０を用いたシミュレーションの結果を示す図である。具体的に、図７には、湾曲可能部１１０の湾曲角度θ₁を９０（ｄｅｇ）とし、湾曲可能部１１０の旋回角度ζ₁を０（ｄｅｇ）とし、湾曲可能部１１０のねじれ角度τ₁を１８０（ｄｅｇ）とするときの湾曲可能部１１０のモデルが示されている。

図７において、実線は、本発明の第１の実施形態に係る連続体ロボット制御システム１０のモデルを用いて導出したワイヤ７１０（以下、「本実施形態のワイヤ７１０」と記載する）の形状を表している。また、比較例として、破線は、特許文献１の演算方法を用いて導出したワイヤ７２０（以下、「特許文献１のワイヤ７２０」と記載する）の形状を表し、また、点線は、ねじれを考慮せずに導出したワイヤ７３０（以下、「ねじれ非考慮のワイヤ７３０」と記載する）の形状を表している。

図７に示すように、湾曲可能部１１０の旋回角度ζ₁が０（ｄｅｇ）でねじれ角度τ₁が１８０（ｄｅｇ）のとき、湾曲可能部１１０の近位端に相当する先端面１２０１におけるワイヤ１ａの位置である点Ｐ_1a1,0は、湾曲可能部１１０の内周に位置する。また、湾曲可能部１１０の遠位端１１１におけるワイヤ１ａの位置である点Ｐ_1a1,mは、湾曲可能部１１０の外周上に位置する。図７において実線で示すように、本実施形態のワイヤ７１０は、点Ｐ_1a1,0から点Ｐ_1a1,mまで、湾曲可能部１１０の中心軸１１３に沿ってらせん状に進むことがわかる。一方、特許文献１のワイヤ７２０では、湾曲可能部１１０の近位端に相当する先端面１２０１におけるワイヤ１ａの位置は、点Ｐ_1a1,0をＺ₁軸まわりにねじれ角度として１８０（ｄｅｇ）だけ回転させた点Ｐ''_1a1,0となる。そのため、図７において破線で示すように、特許文献１のワイヤ７２０は、点Ｐ''_1a1,0から点Ｐ_1a1,mまで、湾曲可能部１１０の中心軸１１３に沿って湾曲可能部１１０の外周を進む。また、ねじれ非考慮のワイヤ７３０では、湾曲可能部１１０の遠位端１１１におけるワイヤ１ａの位置は、点Ｐ_1a1,mをＺ_1,m+1軸まわりに－１８０（ｄｅｇ）だけ回転させた点Ｐ''_1a1,mとなる。そのため、図７において点線で示すように、ねじれ非考慮のワイヤ７３０は、点Ｐ_1a1,0から点Ｐ''_1a1,mまで、湾曲可能部１１０の中心軸１１３に沿って湾曲可能部１１０の内周を進む。

図８は、本発明の第１の実施形態に係る連続体ロボット制御システム１０を用いたシミュレーションの結果を示す図である。具体的に、図８には、湾曲可能部１１０の湾曲角度θ₁を９０（ｄｅｇ）とし、湾曲可能部１１０の旋回角度ζ₁を０（ｄｅｇ）として、湾曲可能部１１０のねじれ角度τ₁を０（ｄｅｇ）から１８０（ｄｅｇ）まで変化させたときのワイヤ１ａの長さを示している。

図８において、実線は、図７に示す本実施形態のワイヤ７１０の長さｌ_1a1を表し、破線は、図７に示す特許文献１のワイヤ７２０の長さｌ_{1a1_pr}を表し、点線は、図７に示すねじれ非考慮のワイヤ７３０の長さｌ_{1a1_wo}を表している。図８において実線で示すように、本実施形態のワイヤ７１０は、ねじれ角度τ₁が増加すると、らせんのピッチが減少するため、そのワイヤの長さｌ_1a1が増加することがわかる。また、図８において破線で示すように、ねじれ角度τ₁が０（ｄｅｇ）から１８０（ｄｅｇ）の間にあるとき、特許文献１のワイヤ７２０は、本実施形態のワイヤ７１０よりも湾曲可能部１１０の外周を進む。このため、特許文献１のワイヤ７２０の長さｌ_{1a1_pr}は、本実施形態のワイヤ７１０の長さｌ_1a1と比べて大きくなることがわかる。さらに、図８において点線で示すように、ねじれ非考慮のワイヤ７３０は、ねじれ角度τ₁によらずに湾曲可能部１１０の内周を進むため、そのワイヤの長さｌ_{1a1_wo}は、本実施形態のワイヤ７１０の長さｌ_1a1と比べて小さくなることがわかる。

第１の実施形態に係る連続体ロボット制御システム１０では、ワイヤ長演算部６２０において、湾曲可能部１１０を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度Δθ、旋回角度ζ及びねじれ角度τに基づいて、当該微小区間におけるワイヤ１ａの長さｌ_1a1,j-1、ｌ_1a1,j、ｌ_1a1,j+1、・・・を演算するようにしている。そして、加算部６３０において、ワイヤ長演算部６２０で得られた複数の微小区間におけるワイヤ１ａの長さｌ_1a1,j-1、ｌ_1a1,j、ｌ_1a1,j+1、・・・を加算することによって、湾曲可能部１１０におけるワイヤ１ａの長さｌ_1a1を算出するようにしている。
かかる構成によれば、連続体ロボット１００－１の湾曲可能部１１０にねじれが生じた場合にも、湾曲可能部１１０におけるワイヤ１ａの長さを精度良く算出することができる。湾曲可能部１１０を湾曲させるための他のワイヤ１ｂ及び１ｃについても、ワイヤ１ａの場合と同様の処理を行うことにより、湾曲可能部１１０にねじれが生じた場合にも、湾曲可能部１１０におけるワイヤ１ｂ及び１ｃの長さを精度良く算出することができる。これにより、連続体ロボット１００－１の制御性能の向上を実現することができる。さらに、第１の実施形態に係る連続体ロボット制御システム１０における上述した構成によれば、湾曲可能部１１０にねじれが生じた場合にも、湾曲可能部１１０の目標姿勢と実際の姿勢との誤差を低減し、安全性や操作性を向上することが可能となる。

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

＜２－１．モデリング＞
上述した第１の実施形態では、立体的に湾曲可能な湾曲可能部１１０を１つ備える連続体ロボット１００－１を想定した形態であったが、第２の実施形態では、湾曲可能部１１０を複数（２つ）備える連続体ロボット１００を想定した形態である。

図９は、本発明の第２の実施形態で用いる連続体ロボット１００の外観構成の一例を示す図である。ここで、以下の説明では、この図９に示す第２の実施形態で用いる連続体ロボット１００を「連続体ロボット１００－２」と記載する。なお、図９において、図１及び図２に示す構成と同様の構成については同じ符号を付しており、また、図９には、第２の実施形態で用いる連続体ロボット１００－２のうち、２つの湾曲可能部１１０－１及び１１０－２の部分のみを図示しているものとする。即ち、第２の実施形態で用いる連続体ロボット１００－２には、図９に示す２つの湾曲可能部１１０－１及び１１０－２に加えて、図１に示す長尺部１２０及びアクチュエータ１３０に相当する構成部も構成されているものとする。

具体的に、図９に示す連続体ロボット１００－２は、互いに直列に設けられた複数の湾曲可能部として、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２を備えて構成されている。第１の湾曲可能部１１０－１は、図１及び図２に示す湾曲可能部１１０に対応する湾曲可能部である。第２の湾曲可能部１１０－２は、第１の湾曲可能部１１０－１よりも駆動部であるアクチュエータ１３０から遠い位置に設けられた湾曲可能部である。なお、本実施形態においては、複数の湾曲可能部として、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２の２つの湾曲部可能部を備える例を示しているが、３つ以上の湾曲部可能部を備える態様も適用可能である。

第１の湾曲可能部１１０－１は、ワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃのうちの少なくとも１つのワイヤが駆動することによって立体的に湾曲可能な構成部である。第１の湾曲可能部１１０－１の遠位端１１１には、第１の湾曲可能部１１０－１の中心軸周りのねじれ量を示すねじれ角度τ₁を検出するための小型のセンサコイル１１１１が設けられている。図９に示すワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃは、図９に示す複数のワイヤガイド１１２と図１に示す長尺部１２０に設けられた孔に案内される。ここで、本実施形態では、図１に示す長尺部１２０の先端面１２０１を近位端とし、当該近位端からからセンサコイル１１１１が設けられている遠位端１１１までが、第１の湾曲可能部１１０－１であるものとする。そして、図９に示すワイヤ１ａ、ワイヤ１ｂ及びワイヤ１ｃは、一端が図９に示す第１の湾曲可能部１１０－１の遠位端１１１（センサコイル１１１１が設けられているワイヤガイド）に接続され、他端が図１に示す対応するアクチュエータ１３０－１ａ～１３０－１ｃに接続される。本実施形態では、アクチュエータ１３０－１ａ～１３０－１ｃを第１の湾曲可能部１１０－１の中心軸方向に駆動すると、ワイヤ１ａ～１ｃがそれぞれ押し引きされ、第１の湾曲可能部１１０－１が立体的に湾曲する。

第２の湾曲可能部１１０－２は、ワイヤ２ａ、ワイヤ２ｂ及びワイヤ２ｃのうちの少なくとも１つのワイヤが駆動することによって立体的に湾曲可能な構成部である。第２の湾曲可能部１１０－２の遠位端１２１には、第２の湾曲可能部１１０－１の中心軸周りのねじれ量を示すねじれ角度τ₂を検出するための小型のセンサコイル１２１１が設けられている。図９に示すワイヤ２ａ、ワイヤ２ｂ及びワイヤ２ｃは、図９に示す複数のワイヤガイド１２２と、第１の湾曲可能部１１０－１の遠位端１１１及び複数のワイヤガイド１１２と、図１に示す長尺部１２０に設けられた孔に案内される。即ち、本実施形態では、第１の湾曲可能部１１０－１の遠位端１１１及び複数のワイヤガイド１１２には、ワイヤ１ａ～１ｃの案内孔に加えて、ワイヤ２ａ～２ｃを案内するための孔が設けられている。ここで、本実施形態では、図９に示す第１の湾曲可能部１１０－１の遠位端１１１を近位端とし、当該近位端からからセンサコイル１２１１が設けられている遠位端１２１までが、第２の湾曲可能部１１０－２であるものとする。そして、図９に示すワイヤ２ａ、ワイヤ２ｂ及びワイヤ２ｃは、一端が図９に示す第２の湾曲可能部１１０－２の遠位端１２１（センサコイル１２１１が設けられているワイヤガイド）に接続され、他端が図１に示すアクチュエータ１３０－１ａ～１３０－１ｃに対応する不図示のアクチュエータ１３０－２ａ～１３０－２ｃに接続される。本実施形態では、この不図示のアクチュエータ１３０－２ａ～１３０－２ｃを第２の湾曲可能部１１０－２の中心軸方向に駆動すると、ワイヤ２ａ～２ｃがそれぞれ押し引きされ、第２の湾曲可能部１１０－２が立体的に湾曲する。

図１０は、図９に示す第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２のモデルの一例を示す図である。この図１０において、図３及び図９に示す構成要素と同様の構成要素については同じ符号を付しており、その詳細な説明は省略する。

図１０では、第１の湾曲可能部１１０－１の遠位端１１１を原点Ｏ₂とし、この遠位端１１１の法線方向にＺ₂軸をとり、円周方向にＸ₂軸をとり、Ｘ₂軸とＺ₂軸に直交するようにＹ₂軸をとる。そして、Ｘ₂Ｙ₂平面内で原点Ｏ₂から第２の湾曲可能部１１０－２の遠位端１２１の中心点Ｏ₃に向かう方向にＷ₂軸をとり、Ｘ₂軸とＷ₂軸とのなす角度を第２湾曲可能部の旋回角度~ζ₂とする。ここで、第２湾曲可能部の旋回角度~ζ₂における符号「～」は、相対座標系であることを示している。また、以下の数式では、相対座標系であることを示す符号（～）を各要素の上に付しているが、本明細書に記載の文章中では、書式の関係から旋回角度「~ζ₂」等のように横にずらした記載としており、以降の説明では、これらは同じものを示しているものとする。

図１０では、第２の湾曲可能部１１０－２の遠位端１２１の中心点Ｏ₃を原点とし、この遠位端１２１の法線方向にＺ₃軸をとる。そして、Ｚ₂軸とＺ₃軸とのなす角度を、第１の湾曲可能部１１０－１に対する第２の湾曲可能部１１０－２の相対湾曲角度~θ₂とする。

本実施形態では、ワイヤ２ａのうち、第１の湾曲可能部１１０－１を通過する部分の長さｌ_2a1と第２の湾曲可能部１１０－２を通過する部分の長さｌ_2a2を、第１の実施形態と同様の方法を用いて演算する。そして、本実施形態では、長さｌ_2a1と長さｌ_2a2を加算して、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２におけるワイヤ２ａの全体の長さｌ_2aを算出する。

まず、第１の湾曲可能部１１０－１におけるワイヤ２ａの長さ（第１の湾曲可能部１１０－１を通過するワイヤ２ａの長さ）ｌ_2a1を導出する。第１の湾曲可能部１１０－１のｊ番目の微小区間におけるワイヤ２ａの長さｌ_2a1,jは、原点Ｏ_1,jから近位端である先端面１２０１へ向かうベクトルｐ_2a1,jと遠位端１１１へ向かうベクトルｐ_2a1,j+1との差となる。したがって、第１の実施形態の（１１）式と同様に、ｌ_2a1,jは、以下の（２１）式で表される。

ワイヤ１ａに対するワイヤ２ａの位相をξ₂とすると、ベクトルｐ_2a1,jは、（１）式と同様に、以下の（２２）式で表される。

また、ベクトルｐ_2a1,j+1は、原点Ｏ_1,jから、点Ｐ_2a1,j+1をＸ_1,jＹ_1,j平面へ射影した点Ｐ'_2a1,jへのベクトルｐ_2a1,jを用いて、（８）式と同様に、以下の（２３）式で表される。

また、ベクトルｐ'_2a1,jは、（２）式と同様に、以下の（２４）式で表される。

そして、長さｌ_2a1,jを、以下の（２５）式を用いて全ての微小区間について加算することで、長さｌ_2a1を得る。

次に、第２の湾曲可能部１１０－２におけるワイヤ２ａの長さ（第２の湾曲可能部１１０－２を通過するワイヤ２ａの長さ）ｌ_2a2を導出する。第２の湾曲可能部１１０－２のｋ番目（ｋ＝１，２，…，ｍ₂）の微小区間におけるワイヤ２ａの長さｌ_2a2,kは、微小区間の原点Ｏ_2,kから近位端１１１へ向かうベクトルｐ_2a2,kと遠位端１２１へ向かうベクトルｐ_2a2,k+1との差となるため、（１１）式と同様に、以下の（２６）式で表される。

ベクトルｐ_2a2,kは、（１）式と同様に、ｋ番目の微小区間のねじれ角度τ_2,kを用いて、以下の（２７）式で表される。

また、ベクトルｐ_2a2,k+1は、原点Ｏ_2,kから、点Ｐ_2a2,k+1をＸ_2,kＹ_2,k平面へ射影した点Ｐ'_2a2,kへのベクトルｐ_2a2,kと、湾曲角度~θ_2,kを用いて、（８）式と同様に、以下の（２８）式で表される。

また、ベクトルｐ'_2a2,kは、（２）式と同様に、以下の（２９）式で表される。

ただし、第１の実施形態の（３）式～（７）式と同様に、本実施形態では、ｔ_2,k、Δｌ_2,k、Δ~θ_2,k、τ_2,kを、それぞれ、以下の（３０）式～（３４）式とする。

そして、長さｌ_2a2,kを、以下の（３５）式を用いて全ての微小区間について加算することで、長さｌ_2a2を得る。

そして、以下の（３６）式により、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２におけるワイヤ２ａの全体の長さｌ_2aを算出する。

他のワイヤ２ｂ及び２ｃについても、ワイヤ２ａと同様に、第１の湾曲可能部１１０－１を通過する部分の長さと第２の湾曲可能部１１０－２を通過する部分の長さをそれぞれ演算し、それらを加算することで、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２におけるワイヤ２ｂ及び２ｃの全体の長さｌ_2b及びｌ_2cを算出することができる。

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

連続体ロボット制御システム１０－２は、図１１に示すように、連続体ロボット１００（より具体的には、図９に示す連続体ロボット１００－２）、連続体ロボット制御装置２００、入力装置３１０、及び、入力装置３２０を有して構成されている。即ち、連続体ロボット制御システム１０－２は、ワイヤ１ａ～１ｃが駆動することによって湾曲する第１の湾曲可能部１１０－１と、ワイヤ２ａ～２ｃが駆動することによって湾曲する第２の湾曲可能部１１０－２と、ワイヤ１ａ～１ｃを駆動するアクチュエータ（駆動部）１３０－１ａ～１３０－１ｃと、ワイヤ２ａ～２ｃを駆動するアクチュエータ（駆動部）１３０－２ａ～１３０－２ｃを備える連続体ロボット１００－２の制御システムである。

図１１に示す連続体ロボット制御システム１０－２では、図５に示す連続体ロボット制御システム１０－１に対して、まず、入力装置３２０が追加されて構成されている。

入力装置３１０の入力部３１１は、連続体ロボット制御装置２００に対して、第１の湾曲可能部１１０－１の湾曲角度（目標湾曲角度）θ₁及び第１の湾曲可能部１１０－１の旋回角度（目標旋回角度）ζ₁を入力する構成部である。入力装置３１０の磁気センサ部３１２は、第1の湾曲可能部１１０－１の遠位端１１１に設けられているセンサコイル１１１１で得られた磁界の強さ・向きに基づいて、第1１の湾曲可能部１１０－１の中心軸１１３周りのねじれ角度τ₁を計測し、これを第1の湾曲可能部１１０－１のねじれ角度τ₁として連続体ロボット制御装置２００に入力する構成部である。ここで、本実施形態においては、磁気センサ部３１２及びセンサコイル１１１１は、湾曲可能部１１０のねじれ角度τ₁を取得する「ねじれ角度取得手段」を構成する。

入力装置３２０は、連続体ロボット制御装置２００に対して情報を入力する装置であり、入力部３２１及び磁気センサ部３２２を有して構成されている。入力部３２１は、連続体ロボット制御装置２００に対して、第２の湾曲可能部１１０－２の湾曲角度（目標湾曲角度）θ₂及び第２の湾曲可能部１１０－２の旋回角度（目標旋回角度）ζ₂を入力する構成部である。磁気センサ部３２２は、第２の湾曲可能部１１０－２の遠位端１２１に設けられているセンサコイル１２１１で得られた磁界の強さ・向きに基づいて、第２の湾曲可能部１１０－２の中心軸１１３周りのねじれ角度τ₂を計測し、これを第２の湾曲可能部１１０－２のねじれ角度τ₂として連続体ロボット制御装置２００に入力する構成部である。ここで、本実施形態においては、磁気センサ部３２２及びセンサコイル１２１１は、湾曲可能部１１０のねじれ角度τ₂を取得する「ねじれ角度取得手段」を構成する。

図１１に示す連続体ロボット制御装置２００は、入力装置３１０及び入力装置３２０から入力された情報に基づいて、連続体ロボット１００（より具体的には、図９に示す連続体ロボット１００－２）を制御する装置である。この図１１に示す連続体ロボット制御装置２００は、運動学演算部２１１～２１３、位置制御部２２１～２２２、及び、加算部２３１を有して構成されている。ここで、図１１に示す運動学演算部２１１及び位置制御部２２１は、それぞれ、図５に示す運動学演算部２１１及び位置制御部２２１と同様の処理を行うものであるため、その説明は省略する。

運動学演算部２１２は、入力装置３１０から入力された情報である第１の湾曲可能部１１０－１の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁に基づいて、（２５）式を用いて、第１の湾曲可能部１１０－１におけるワイヤ２ａ，２ｂ及び２ｃの長さｌ_2a1，ｌ_2b1及びｌ_2c1を演算する構成部である。また、運動学演算部２１３は、入力装置３２０から入力された情報である第２の湾曲可能部１１０－２の湾曲角度θ₂、旋回角度ζ₂及びねじれ角度τ₂に基づいて、（３５）式を用いて、第２の湾曲可能部１１０－２におけるワイヤ２ａ，２ｂ及び２ｃの長さｌ_2a2，ｌ_2b2及びｌ_2c2を演算する構成部である。

加算部２３１は、運動学演算部２１２で算出された第１の湾曲可能部１１０－１におけるワイヤ２ａ，２ｂ及び２ｃの長さｌ_2a1，ｌ_2b1及びｌ_2c1と、運動学演算部２１３で算出された第２の湾曲可能部１１０－２におけるワイヤ２ａ，２ｂ及び２ｃの長さｌ_2a2，ｌ_2b2及びｌ_2c2とを、各ワイヤごとに加算することによって、第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２におけるワイヤ２ａ～２ｃの全体の長さｌ_2a～ｌ_2cを算出する。

位置制御部２２２は、第２の湾曲可能部１１０－２におけるワイヤ２ａ，２ｂ及び２ｃの長さが、それぞれ、加算部２３１によって算出された長さｌ_2a，ｌ_2b及びｌ_2cとなるように、連続体ロボット１００－２のアクチュエータ１３０－２ａ，１３０－２ｂ及び１３０－２ｃに対して駆動指令ｆ_2a，ｆ_2b及びｆ_2cを出力する構成部である。即ち、位置制御部２２２は、運動学演算部２１２及び運動学演算部２１３で得られた第１の湾曲可能部１１０－１及び第２の湾曲可能部１１０－２におけるワイヤ２ａ～２ｃの長さｌ_2a1，ｌ_2b1，ｌ_2c1、及び、ｌ_2a2，ｌ_2b2，ｌ_2c2に基づいて、アクチュエータ１３０－２ａ～１３０－２ｃによるワイヤ２ａ～２ｃの駆動を制御する制御手段である。

なお、本実施形態では、２つの湾曲可能部１１０－１～１１０－２を備える連続体ロボット１００－２を想定した例を示したが、一般に、３以上のｎ個の湾曲可能部を備える連続体ロボットにおいても同様である。即ち、第ｉ（ｉ＝１，２，…，ｎ）の湾曲可能部のワイヤの全長は、第１の湾曲可能部から第ｉの湾曲可能部を通過するワイヤの長さをそれぞれ演算するｉ個の運動学演算部と、それらの出力を加算する加算部を用いて導出されることになる。

以上説明した第２の実施形態によれば、連続体ロボット１００－２の複数の湾曲可能部１１０にねじれが生じた場合にも、１つ及び複数の湾曲可能部１１０におけるワイヤの長さを精度良く算出することができる。これにより、連続体ロボット１００－２の制御性能の向上を実現することができる。さらに、連続体ロボット１００－２の複数の湾曲可能部１１０にねじれが生じた場合にも、１つ及び複数の湾曲可能部１１０の目標姿勢と実際の姿勢との誤差を低減し、安全性や操作性を向上することが可能となる。

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

第３の実施形態では、湾曲可能部１１０の湾曲角度θ、旋回角度ζ及びねじれ角度τの分布が一様とならない連続体ロボット１００に対する制御システムを適用する。

第１の実施形態と第２の実施形態では、（７）式と（３４）式に示すように、湾曲可能部１１０のねじれ量であるねじれ角度τが一様に分布すると仮定して、各微小区間のねじれ角度τを導出している。同様に、第１の実施形態と第２の実施形態では、（５）式と（３２）式に示すように、湾曲可能部１１０は、曲率が一定となるように湾曲すると仮定して、各微小区間の湾曲角度Δθを導出している。しかしながら、大腸や肺などの器官では、その経路が複雑に湾曲するため、これらの器官に侵入する湾曲可能部１１０では、上述の仮定が常に成立しているとは限らない。このような場合には、湾曲可能部１１０の侵入経路の形状と一致するように、各微小区間の湾曲角度θ、旋回角度ζ及びねじれ角度τを求め、これらに基づいて湾曲可能部１１０における各ワイヤの長さを演算することで、連続体ロボット１００の制御性能を向上させることができる。そこで、本実施形態の制御系では、まず、連続体ロボット１００の湾曲可能部１１０の遠位端が狭小空間上のある点を通過するときの湾曲角度θ、旋回角度ζ及びねじれ角度τを記憶する。そして、湾曲可能部１１０の微小区間がその点に到達すると、これらの角度を読み出して、湾曲可能部１１０における各ワイヤの長さを演算するために用いる。

ここで、以下に記載する第３の実施形態の説明では、上述した第１の実施形態と異なる事項について説明を行う。

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

図１２に示す連続体ロボット制御システム１０－３は、湾曲可能部１１０、長尺部１２０、アクチュエータ・制御系内蔵ボックス１２１０、直動案内機構部１２２０、及び、位置センサ部１２３０を有して構成されている。

アクチュエータ・制御系内蔵ボックス１２１０は、例えば、図１に示すアクチュエータ１３０－１ａ～１３０－１ｃや、図５に示す連続体ロボット制御装置２００及び入力装置３１０等を内蔵する構成部である。

直動案内機構部１２２０は、長尺部１２０及び湾曲可能部１１０が取り付けられたアクチュエータ・制御系内蔵ボックス１２１０を直動可能に案内する構成部である。具体的に、直動案内機構部１２２０は、湾曲可能部１１０の長手方向に前後進する。

位置センサ部１２３０は、連続体ロボット１００の湾曲可能部１１０における前後進方向の位置ｚ_bを検出する。より具体的に、位置センサ部１２３０は、湾曲可能部１１０の初期位置からの変位ｚ_bを検出する。

第３の実施形態に係る連続体ロボット制御システム１０は、図５に示す第１の実施形態に係る連続体ロボット制御システム１０－１と同様である。

図１３は、本発明の第３の実施形態を示し、図５に示す運動学演算部２１１の機能構成の一例を示す図である。この図１３において、図６に示す構成と同様の構成については同じ符号を付しており、その詳細な説明は省略する。

図１３の第３の実施形態における運動学演算部２１１は、図５の第１の実施形態における運動学演算部２１１に対して、図１２に示す位置センサ部１２３０で検出された湾曲可能部１１０の初期位置からの変位ｚ_bが角度演算部６１０に更に入力される点が異なる。即ち、第３の実施形態では、角度演算部６１０は、入力装置３１０から入力された情報である湾曲可能部１１０の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁に加えて、位置センサ部１２３０で検出された湾曲可能部１１０の初期位置からの変位ｚ_bに基づいて、湾曲可能部１１０を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度Δθ、旋回角度ζ及びねじれ角度τを演算する。

長尺部１２０及び湾曲可能部１１０が取り付けられたアクチュエータ・制御系内蔵ボックス１２１０が直動案内機構部１２２０を前後進するとき、角度演算部６１０は、位置センサ部１２３０から湾曲可能部１１０の初期位置からの変位ｚ_bを取得して記憶するとともに、入力装置３１０から、変位ｚ_bに対応する湾曲可能部１１０の湾曲角度θ₁、旋回角度ζ₁及びねじれ角度τ₁を取得して記憶する。

アクチュエータ・制御系内蔵ボックス１２１０が直動案内機構部１２２０の初期位置から或る位置に移動して変位ｚ_bになる場合に、湾曲可能部１１０のｊ番目の微小区間の近位端側の位置ｚ_b1,jは、以下の（３７）式で表される。

そこで、角度演算部６１０は、以下の（３８）式～（４０）式を用いて、湾曲可能部１１０のｊ番目の微小区間における湾曲角度Δθ_1,j、旋回角度ζ_1,j及びねじれ角度τ_1,jを演算する。

これにより、ワイヤ長演算部６２０は、湾曲可能部１１０が狭小空間に沿って位置ｚ_b1,jを通過するときの湾曲角度Δθ、旋回角度ζ及びねじれ角度τに基づいて、微小区間のワイヤの長さを演算することが可能となる。そのため、複雑に湾曲する狭小空間に湾曲可能部１１０が侵入するときにも連続体ロボット１００の制御性能を向上させることができる。

なお、本実施形態では、ねじれ角度τが非一様に分布する要因として狭小空間からの外力を考慮する例を示したが、外力以外の要因を考慮して、各ワイヤの長さを演算することも可能である。例えば、湾曲可能部１１０におけるねじれ剛性が非一様に分布する連続体ロボット１００に対しては、ｊ番目の微小区間のねじれ剛性に対応する係数α_1,jを用いて、以下の（４１）式を用いて、ねじれ角度τ_1,jを演算すれば、剛性の分布を考慮してワイヤの長さを演算することが可能となる。

これは、湾曲角度θや旋回角度ζについても同様である。

（その他の実施形態）
上述した第１～第３の実施形態では、入力装置に構成した入力部や磁気センサ部を用いて湾曲可能部１１０の湾曲角度や旋回角度、ねじれ角度を取得する制御系の例を示したが、本発明においてはこの形態に限定されるものではない。例えば、湾曲可能部１１０の変形量を計測する変位センサや、湾曲可能部１１０の形状を撮影した画像情報を用いてこれらの角度を取得する形態も、本発明に適用可能である。また、湾曲可能部１１０が侵入する狭小空間の形状から、湾曲可能部１１０の湾曲角度や旋回角度、ねじれ角度を演算して取得する形態も、本発明に適用可能である。

また、上述した第１～第３の実施形態では、１つの湾曲可能部１１０を湾曲させるためのワイヤとして３本のワイヤの押し引きにより湾曲可能部１１０を湾曲させる例を示したが、本発明においてはこの形態に限定されるものではない。即ち、上述した第１～第３の実施形態では、１つの湾曲可能部１１０を立体駆動する連続体ロボット１００の例を示したが、平面上で湾曲する連続体ロボットや、ワイヤの本数が異なる連続体ロボットに対しても、本発明を適用することが可能である。

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

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

１ａ～１ｃ：ワイヤ、１０：連続体ロボット制御システム、１００：連続体ロボット、１１０：湾曲可能部、１２０：長尺部、１３０：アクチュエータ、２００：連続体ロボット制御装置、２１１：運動学演算部、２２１：位置制御部、３１０：入力装置、３１１：入力部、３１２：磁気センサ部、６１０：角度演算部、６２０：ワイヤ長演算部、６２１：ワイヤ長演算器、６３０：加算部

Claims (7)

1. ワイヤが駆動することによって湾曲する湾曲可能部と、前記ワイヤを駆動する駆動部と、を備える連続体ロボットの制御システムであって、
   前記湾曲可能部のねじれ角度を取得するねじれ角度取得手段と、
   前記ねじれ角度取得手段によって取得されたねじれ角度に基づいて、前記湾曲可能部における前記ワイヤの長さを演算する運動学演算手段と、
   を有し、
   前記運動学演算手段は、
   前記湾曲可能部を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度、旋回角度およびねじれ角度に基づいて、当該微小区間における前記ワイヤの長さを演算するワイヤ長演算手段と、
   前記ワイヤ長演算手段で得られた前記複数の微小区間における前記ワイヤの長さを加算することによって、前記湾曲可能部における前記ワイヤの長さを算出する加算手段と、
   を有することを特徴とする連続体ロボットの制御システム。
2. 前記湾曲可能部の湾曲角度および旋回角度、並びに、前記ねじれ角度取得手段によって取得されたねじれ角度に基づいて、前記複数の微小区間におけるそれぞれの微小区間ごとの湾曲角度、旋回角度およびねじれ角度を演算する角度演算手段を更に有し、
   前記ワイヤ長演算手段は、前記角度演算手段で得られた、前記複数の微小区間におけるそれぞれの微小区間ごとの湾曲角度、旋回角度およびねじれ角度を用いて、当該微小区間における前記ワイヤの長さを演算することを特徴とする請求項１に記載の連続体ロボットの制御システム。
3. 前記角度演算手段は、前記湾曲可能部の湾曲角度および旋回角度、並びに、前記ねじれ角度取得手段によって取得されたねじれ角度に加えて、前記湾曲可能部の初期位置からの変位に基づいて、前記複数の微小区間におけるそれぞれの微小区間ごとの湾曲角度、旋回角度およびねじれ角度を演算することを特徴とする請求項２に記載の連続体ロボットの制御システム。
4. 前記運動学演算手段で得られた前記湾曲可能部における前記ワイヤの長さに基づいて、前記駆動部による前記ワイヤの駆動を制御する制御手段を更に有することを特徴とする請求項１乃至３のいずれか１項に記載の連続体ロボットの制御システム。
5. 前記連続体ロボットは、互いに直列に設けられた複数の前記湾曲可能部であって、第１の湾曲可能部および前記第１の湾曲可能部よりも前記駆動部から遠い位置に設けられた第２の湾曲可能部を含む複数の湾曲可能部を備えて構成されており、
   前記ねじれ角度取得手段は、前記第１の湾曲可能部の前記ねじれ角度および前記第２の湾曲可能部の前記ねじれ角度を取得し、
   前記運動学演算手段は、前記ねじれ角度取得手段によって取得された前記第１の湾曲可能部のねじれ角度に基づいて、前記第１の湾曲可能部を湾曲させるための前記ワイヤである第１のワイヤの前記第１の湾曲可能部における長さを演算するとともに、前記第２の湾曲可能部を湾曲させるための前記ワイヤである第２のワイヤの前記第１の湾曲可能部における長さを演算し、且つ、前記ねじれ角度取得手段によって取得された前記第２の湾曲可能部のねじれ角度に基づいて、前記第２のワイヤの前記第２の湾曲可能部における長さを演算し、
   前記加算手段とは異なる第２の加算手段であって、前記運動学演算手段で得られた前記第１の湾曲可能部における前記第２のワイヤの長さと前記第２の湾曲可能部における前記第２のワイヤの長さとを加算することによって、前記第１の湾曲可能部および前記第２の湾曲可能部における前記第２のワイヤの長さを算出する第２の加算手段を更に有することを特徴とする請求項１乃至４のいずれか１項に記載の連続体ロボットの制御システム。
6. ワイヤが駆動することによって湾曲する湾曲可能部と、前記ワイヤを駆動する駆動部と、前記駆動部を制御する制御装置とを備える連続体ロボットの作動方法であって、
   前記制御装置は、
   前記湾曲可能部のねじれ角度を取得するねじれ角度取得ステップと、
   前記ねじれ角度取得ステップによって取得されたねじれ角度に基づいて、前記湾曲可能部における前記ワイヤの長さを演算する運動学演算ステップと、
   を実行し、
   前記運動学演算ステップは、
   前記湾曲可能部を長手方向に分割することによって得られる複数の微小区間におけるそれぞれの微小区間ごとに、当該微小区間の湾曲角度、旋回角度およびねじれ角度に基づいて、当該微小区間における前記ワイヤの長さを演算するワイヤ長演算ステップと、
   前記ワイヤ長演算ステップで得られた前記複数の微小区間における前記ワイヤの長さを加算することによって、前記湾曲可能部における前記ワイヤの長さを算出する加算ステップと、
   を有することを特徴とする連続体ロボットの作動方法。
7. コンピュータを、請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システムにおける各手段として機能させるためのプログラム。

Images