JP7313899B2 - 連続体ロボットおよび連続体ロボットの制御システム - Google Patents

Description

本発明は、連続体ロボットおよび連続体ロボットの制御システムに関する。

連続体ロボットはコンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間からなり、その湾曲区間を変形させることにより形状が制御される。このロボットは、剛体リンクにより構成されるロボットに対して、主に二つの優位性を持つ。一つ目は、剛体リンクロボットがはまり込んでしまうような、狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。二つ目は、連続体ロボットは本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく対象物内を移動することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、内視鏡のシースやカテーテルなど医療の分野や、レスキューロボットなど極限作業ロボットへの応用が期待されている。連続体ロボットの駆動方法として、腱駆動によるもの、押引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。非特許文献１には、３本のワイヤと、それをガイドするスペーサーディスクと呼ばれるワイヤガイド、エンドディスクと呼ばれる遠位端、および基台部により構成される連続体ロボットが開示されている。運動学を導出することでワイヤの押引き駆動量を求め、連続体ロボットの形状の制御を行っている。非特許文献１に記載されたロボットでは、それぞれのワイヤガイドの円周に沿ってワイヤが挿通される孔が設けられている。ワイヤは遠位端であるエンドディスクにのみ固定され、基台部とエンドディスクとの間に設けられたワイヤガイドに対して摺動する構造である。

Ｋ．Ｘｕ，Ｍ．Ｆｕ，ａｎｄ Ｊ．Ｚｈａｏ，"Ａｎ Ｅｘｐｅｒｉｍｅｎｔａｌ Ｋｉｎｅｓｔａｔｉｃ Ｃｏｍｐａｒｉｓｏｎ ｂｅｔｗｅｅｎ Ｃｏｎｔｉｎｕｕｍ Ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ Ｓｓｐａｎｕｃｔｕｒａｌ Ｖａｒｉａｔｉｏｎｓ，"ｉｎ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ），Ｈｏｎｇ Ｋｏｎｇ，Ｃｈｉｎａ，２０１４，ｐｐ．３２５８－３２６４．

非特許文献１において、連続体ロボットの運動を表現するための運動学の導出においては、ワイヤは一様な曲率で変形することを仮定し、さらに、連続体ロボットの中心を通る仮想的なワイヤの長さが、連続体ロボットの形状に依らず常に一定となるようにワイヤの駆動量を求めている。しかし、ワイヤの駆動によるワイヤとワイヤガイド等との間の摺動摩擦によりワイヤガイドの位置が変動すると、ワイヤ曲率が非一様となり、それを一様と仮定する運動学との誤差が大きくなる。そのため、形状制御の精度が不十分となるおそれがある。

上記課題に鑑みて、本発明は、連続体ロボットの制御の精度を向上させることを目的とする。

上記課題を解決する本発明の一の側面である連続体ロボットは、第１ワイヤと、第２ワイヤと、前記第１ワイヤ及び前記第２ワイヤを保持するディスタルガイドと、前記第１ワイヤ及び前記第２ワイヤに対して摺動可能なプロクシマルガイドと、前記ディスタルガイドと前記プロクシマルガイドとの間に設けられた複数のワイヤガイドと、前記第１ワイヤ及び前記第２ワイヤとを駆動する駆動部と、前記駆動部を制御する制御部と、を有し、前記第１ワイヤは、前記複数のワイヤガイドに対して固定され、前記第２ワイヤは、前記複数のワイヤガイドに対して摺動可能であり、前記制御部は、前記複数のワイヤガイドのうち、前記プロクシマルガイドと最も近接して設けられたワイヤガイドと、前記プロクシマルガイドとの間の距離が一定となるように、前記駆動部を制御することを特徴とする。

本発明の別の一の側面である連続体ロボットの制御システムは、基準面を介して延伸する第１および第２のワイヤと、前記第１および第２のワイヤが異なる位置に固定された第１のワイヤガイドと、前記基準面と前記第１のワイヤガイドとの間に前記第１および第２のワイヤを案内する第２のワイヤガイドと、を有し、前記第１および第２のワイヤの少なくとも一方を駆動することにより湾曲可能な湾曲部を、有する連続体ロボットと、前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記第１および第２のワイヤの少なくとも一方の第１の駆動変位量を算出する第１の算出手段と、を有することを特徴とする連続体ロボットの制御システムにおいて、前記第２のワイヤガイドは、前記第１のワイヤが固定され、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との目標距離に応じて、前記第１および第２のワイヤの少なくとも一方の第２の駆動変位量を算出する第２の算出手段と、前記第１の駆動変位量と前記第２の駆動変位量とを加算する加算手段と、を有することを特徴とする。

本発明によれば、連続体ロボットの制御精度を向上させることが可能となる。

実施形態１に係る連続体ロボットの運動学モデルを示す図である。 実施形態１に係る連続体ロボットの運動学モデルを説明するための図である。 実施形態１に係る連続体ロボットの制御システムを示すブロック線図である。 実施形態１に係るシミュレーション応答を示す図である。 実施形態１に係るシミュレーション応答を示す別の図である。 実施形態２に係る連続体ロボットの制御システムを示すブロック線図である。 実施形態３に係る連続体ロボットの制御システムを示すブロック線図である。 実施形態３に係るシミュレーション応答を示す図である。 実施形態４に係る連続体ロボットの運動学モデルを示す図である。 実施形態４に係る連続体ロボットの制御システムを示すブロック線図である。

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

（第１の実施形態）
本実施形態では、３本のワイヤを駆動することで立体的に形状の制御が可能な連続体ロボットの運動学を導出し、最も近位にあるワイヤガイドと基台部の距離を一定とするための制御系を示す。

以下に、連続体ロボットの運動学の導出と制御アルゴリズムについて詳細に記述する。

１．１）モデリング
図１に本実施例で用いる連続体ロボット１００の概略図を示す。ここでは、連続体ロボット１００が１つの湾曲区間を有する例を示す。連続体ロボット１００の湾曲区間の遠位端に設けられた遠位ワイヤガイド（以下、ディスタルガイドとも称す）１６０とワイヤ１１１～１１３とは、接続部１２１～１２３においてそれぞれ接続される。また、連続体ロボット１００は、ワイヤ１１１～１１３を案内するための部材であるワイヤガイド１６１～１６４を有する。最も基端側（近位側）に設けられたワイヤガイド１６４を、近位ワイヤガイドあるいはプロクシマルガイドとも称する。ワイヤガイド１６０～１６４は、円盤状の部材として図示しているが、この例に限らず、たとえば円環状の部材としてもよい。破線で示される連続体ロボット１００の中心軸を含む領域に開口を持つワイヤガイドとすることにより、この開口部にカメラなどのツールを導入することができる。本例のように、複数のワイヤガイドを離散的に配置する方法のほかに、蛇腹状やメッシュ状などの連続体部材をワイヤガイドとして用いても良い。ワイヤガイド１６１～１６４はワイヤ１１１に、固定部１５０～１５３において固定される。一方、ワイヤ１１２、１１３は、ワイヤガイド１６１～１６４に対して摺動可能に構成されている。また、本明細書では、各ワイヤガイドに固定されたワイヤ１１１をａワイヤとよび、ワイヤ１１２，１１３をｘｙ面内においてａワイヤから反時計回りにそれぞれｂワイヤ、ｃワイヤと呼ぶ。ａワイヤは第１ワイヤであり、ｂワイヤおよびｃワイヤは第２ワイヤである。各ワイヤ１１１～１１３のｘｙｚ空間における長さをそれぞれ、ｌ_１ａ，ｌ_１ｂ，ｌ_１ｃとする。また、湾曲区間を駆動するワイヤの駆動変位をｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃとする。ワイヤは図２に示すように、一辺の長さをｒ_ｓとする正三角形の頂点に配置されており、位相角ξ_１は第１湾曲区間を駆動するワイヤ配置を決定する角度である。本実施例では、ξ_１＝０とする。なお、図２は、任意のワイヤガイド１６１～１６４をｘｙ面に載置した際に、連続体ロボット１００の遠位端側から近位端に向かって当該ワイヤガイドを見た平面図である。

ワイヤ１１１～１１３をロボット基台部１４０に設けられた駆動部であるアクチュエータ１３０～１３２で押引き駆動することにより、湾曲区間、すなわち、連続体ロボット１００の姿勢が制御される。ロボット基台部１４０には、ワイヤ１１１～１１３が挿通される開口を有しており、ロボット基台部１４０がワイヤガイドを有するということもできる。このロボット基台部１４０の面を基準面として考えることができる。

記号の定義を以下に示す。ｌ_１ｄ：湾曲区間の中心軸の初期長さ（連続体ロボットが湾曲していない場合の中心軸の長さ）、θ_１：遠位端の湾曲角度（図中ｚ軸に対して遠位ワイヤガイドの法線がなす角度）、ζ_１：遠位端の旋回角度、ρ_１：湾曲区間の曲率半径、ｒ：ワイヤガイドの半径、ｌ_１ａ：ｘｙｚ空間におけるａワイヤの長さ、ｌ_ａｗ１：ａワイヤ先端から最基端側ワイヤガイドの基端側面までのａワイヤ上の距離、ｌ_ｗ１：最基端側ワイヤガイドの基端側面からｘｙ面までのａワイヤ上の距離、ｌ_ｅ１：最基端側ワイヤガイドの基端側面からｘｙ面までの、ｗｚ面内の点Ｃを中心とする半径ρ_１－ｒである円弧上の距離（本発明では、これを近位最小隙間長さとよぶ）、ｌ_ｅｏ１：最基端側ワイヤガイドの基端側面からｘｙ面までの、ｗｚ面内の点Ｃを中心とする半径ρ_１＋ｒである円弧上の距離（本発明では、これを近位最大隙間長さとよぶ）。

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

遠位端の湾曲角度および旋回角度をθ_１，ζ_１とするための、第１湾曲区間のａ，ｂ，ｃワイヤの駆動変位ｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃの候補は、

となる。ここで、例えば図１に示すように、ワイヤガイド１６０～１６４を等間隔に配置すると、ワイヤガイドはａワイヤに固定されているため、旋回角度ζ_１＝０のときａワイヤの負方向の最大駆動変位は、ｔ_ｗｇをワイヤガイド厚さとすると、
－（ｌ_１ｄ－５ｔ_ｗｇ）／５
以下に制約される。また、旋回角度ζ_１に対して、湾曲の最大角度が変動する。さらに、旋回角度ζ_１の変化に伴って基端から基端側のワイヤガイド１６４までの距離が変動する。そのため、連続体ロボットの湾曲区間を被覆するような、例えば樹脂チューブなどを保護構造として連続体ロボット１００に装備した場合、基端から基端側のワイヤガイドまでの保護構造の伸縮量も旋回角度に旋回角度ζ_１に対して変動する。そのため、基端から基端側のワイヤガイド１６４までの距離の変動に対応できるよう、保護構造に大きな伸縮性が求められる。この結果、保護構造として採用できる材料の選択肢が狭まることが懸念される。

そこで、本実施例では、近位最小隙間長さｌ_ｅ１が旋回角度ζ_１に依存せず一定となる制御を行う。この長さは湾曲角度θ_１が正のとき、

となり、湾曲角度θ_１が負のとき、

となる。

１．２）制御系設計
次に、連続体ロボット１００を制御するための制御系について説明する。ここでは、連続体ロボットに対する目標形状、すなわち目標姿勢から、各ワイヤに対する駆動変位を求めると同時に、ワイヤガイドと基台部の距離を一定とするための駆動変位を求める。そして、それを加算するアルゴリズムを用いる。これにより、旋回角度ζ_１に依存せずワイヤガイドと基台部の距離である近位最小隙間長さｌ_ｅ１を一定とすることができる。

図３に、本実施形態に係る連続体ロボット１００の制御部である制御装置３００のブロック線図を示す。ここで、Ｐは連続体ロボット１００を表す。制御装置３００は、ブロックＫおよびブロックＫｉｎｅｍａｔｉｃｓを含んで構成される。Ｋｉｎｅｍａｔｉｃｓは運動学モデルに基づいてワイヤ駆動変位を演算するブロックであり、第１の算出手段に対応する。ブロックＫは第２の算出手段に対応する。入力装置４００は、制御装置３００に対して、近位最少隙間長さｌ_ｅ１、目標角度ベクトルθ_ｒｅｆ１，ζ_ｒｅｆ１を入力する。

目標角度ベクトルθ_ｒｅｆ１，ζ_ｒｅｆ１が設定されると、ブロックＫｉｎｅｍａｔｉｃｓは、ワイヤ駆動変位ｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃを求める。ここで、ブロックＫは、近位最小隙間長さｌ_ｅ１、目標角度ベクトルθ_ｒｅｆ１，ζ_ｒｅｆ１を入力として、ｌ_ｅ１が旋回角度ζ_１に依存せず一定となるための湾曲区間の中心軸の長さｌ_ｄｃを出力する。そして、中心軸初期長さｌ_１ｄからの差分を隙間長さ補償量ｌ_１ｄｄとして、ブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位ｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃにそれぞれ加算する。これにより、近位最小隙間長さがｌ_ｅ１であり、湾曲、旋回角度をθ_１，ζ_１とするワイヤ駆動変位が求められる。

ここで、ブロックＫが中心軸の長さｌ_ｄｃを求めるには、式（２），（３）の中心軸初期長さｌ_１ｄをｌ_１ｄｃと置き換え、中心軸長さｌ_１ｄｃについて解けばよい。湾曲角度θ_１が正のとき、

となり、湾曲角度θ_１が負のとき、

となる。そして、中心軸初期長さｌ_１ｄからの差分を隙間長さ補償量
ｌ_１ｄｄ、ｌ_１ｄｄ＝ｌ_１ｄｃ－ｌ_１ｄ （６）
とする。そして、隙間長さ補償量ｌ_１ｄｄをブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位、ｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃに加算する。これにより、近位最小隙間長さがｌ_ｅ１であり、湾曲、旋回角度をθ_１，ζ_１とするワイヤ駆動変位が求められる。

１．３）シミュレーション
上記の制御系を用いて、連続体ロボット１００を旋回させた場合のシミュレーション結果を説明する。ここでは、連続体ロボット１００の旋回角度ζ_１を０度から１度ステップで３５９度まで変化させ、近位最小隙間長さｌ_ｅ１、近位最大隙間長さｌ_ｅｏ１、さらにはワイヤガイド間の最小、最大隙間長さｌ_ｇｉ，ｌ_ｇｏを求める。ワイヤガイド間の隙間長さは近位隙間長さと同様に、ｗｚ面内の点Ｃを中心とする円弧を用いて求めている。シミュレーションでは、湾曲区間の中心軸初期長さｌ_１ｄを０．０１０ｍとし、ワイヤガイド数を５、ワイヤガイドの厚みを０．０００７５ｍ、ワイヤガイド間の距離を０．００１２５ｍとして均等配置し、近位最小隙間長さｌ_ｅ１が０．００１２５ｍとなるようにワイヤ駆動量を求める。

湾曲角度θ_１を５５度とするシミュレーション応答を図４（ａ）～（ｃ）に示す。本実施形態に係る制御系（以下、近位最小隙間一定制御と略す）による応答を実線で示し、比較として式（１）のみを用いた中心軸長さを一定とする制御（以下、中心軸長一定制御と略す）の応答を破線で示す。

図４（ａ）に近位最小隙間長さｌ_ｅ１の応答を示す。近位最小隙間一定制御では旋回角度ζ_１に依らず一定である。しかし、中心軸長一定制御では旋回角度０度の方向で、ワイヤガイド１６４がａワイヤとともに基台部方向に引き込まれるため、近位最小隙間長さが短くなる。そして、旋回角度１８０度の方向では、近位最小隙間長さが長くなっていることがわかる。つまり、中心軸一定制御では、旋回角度ζ_１に応じて近位最少隙間長さが変動する。

図４（ｂ）に、近位最大隙間長さｌ_ｅｏ１の応答を示す。近位最小隙間一定制御では旋回角度ζ_１に依らずほぼ一定である。しかし、中心軸長一定制御では近位最小隙間長さと同様の応答を示す。

さらに、図４（ｃ）にワイヤガイド間の最小隙間長さｌ_ｇｉの応答を細線で、最大隙間長さｌ_ｇｏの応答を太線で示す。近位最小隙間一定制御によるワイヤガイド間の最小隙間長さの変動幅は中心軸長一定制御による変動幅よりも大きいが、最大隙間長さの変動幅は中心軸長一定制御による変動幅よりも小さいことがわかる。

上記の結果が示すように、本実施形態で提案する近位最小隙間一定制御によれば、中心軸長一定制御とワイヤガイド間の隙間長さの変動幅は同等でありながら、近位最小長さおよび最大隙間長さの旋回角度による変動幅を低減可能であることがわかる。

つぎに、湾曲角度θ_１を９０度とするシミュレーション応答を図５（ａ）～（ｃ）に示す。中心軸初期長さｌ_ｄ１を０．０１０ｍとすると、湾曲角度θ_１が９０度の場合には、中心軸一定制御では近位最小隙間長さが負になってしまい、湾曲区間の湾曲角度の限界を超えるため、中心軸初期長さｌ_ｄ１を０．０１０８ｍとして計算している。図５（ａ）～（ｃ）の結果から分かるように、本条件下においても図４と同様の傾向を示す。このように、近位最小隙間一定制御によれば、中心軸長一定制御とワイヤガイド間の隙間長さの変動幅は同等でありながら、近位最小長さおよび最大隙間長さの旋回角度による変動幅を低減可能である。

本実施形態によれば、連続体ロボットの制御精度を向上させることができる。

（第２の実施形態）
第１の実施形態では、近位最小隙間長さｌ_ｅ１を旋回角度ζ_１に依存せず一定とする制御系を示した。運動学ではワイヤは長手方向に変形しないと仮定しているが、実際は、形状制御によりワイヤが伸縮し、隙間長さ補償の性能が低下する。特に、ワイヤ圧縮時に隙間長さ補償量が負であると、近位端のワイヤガイドが基台部に接触する恐れがある。本実施形態では、このワイヤの伸縮に対して制御系に調整ゲインを導入する。

２．１）モデリング
連続体ロボット１００は、第１の実施形態で説明したものと共通するので、ここでは説明を省略する。

２．２）制御系設計
図６に、本実施形態に係る制御装置３０１のブロック線図を示す。制御装置３０１は、中心軸長さｌ_１ｄｃと中心軸初期長さｌ_１ｄの差分ｌ_ｓｂ１に調整ゲインｒｇ_１を乗じるブロックｒｇ_１を備える点で制御装置３００とは異なっている。ブロックｒｇ_１により、隙間長さ補償量ｌ_１ｄｄは、
ｌ_１ｄｄ＝ｒｇ_１（ｌ_１ｄｃ－ｌ_１ｄ）＝ｒｇ_１ｌ_ｓｂ１ （７）
となる。ここで、調整ゲインｒｇ_１を差分ｌ_ｓｂ１に応じて変動させても良い。例えば、差分ｌ_ｓｂ１正負によって以下のように切り替えてもよい。

これにより実際のワイヤ長の伸縮に対して制御系を調整可能となる。

（第３の実施形態）
第２の実施形態では、ワイヤが伸縮するときに隙間長さの誤差を低減する方法を示した。しかし、連続体ロボットにおけるワイヤの伸縮量は旋回角度ζ_１に依存して非線形に変化するため、（８）式に示すように差分ｌ_ｓｂ１の正負に応じて調整ゲインを切り替える方法では隙間長さを高精度に補償することはできない。そこで、本実施形態では連続体ロボットの静力学モデルを導出し、このモデルを用いてワイヤ伸縮量を演算する。

（モデリング）
本実施形態の説明に用いる記号の定義を以下に示す。
Ｅ：ワイヤのヤング率
Ａ：ワイヤの断面積
Ｉ：ワイヤの断面２次モーメント
Ｌ_０：ａ、ｂ、ｃワイヤの全長
なお、本実施形態において、ワイヤ駆動量ｌ_ｐ１ｂ、ワイヤ伸縮量Δｌ_ａ，Δｌ_ｂ，Δｌ_ｃ，ワイヤ張力ｆ_ａ，ｆ_ｂ，ｆ_ｃは、遠位端方向を正とし、また、モーメントＭ_ａ，Ｍ_ｂ，Ｍ_ｃは、時計方向を正とする。

本実施形態では、ワイヤの伸縮を考慮し、第１の実施形態で示した仮定１、仮定２、および仮定４に加えて、以下に記載する仮定５～仮定１０を前提とした静力学モデルを考える。
［５］：ワイヤガイドとワイヤの間に働く力とモーメントを考慮しない。
［６］：ワイヤと遠位ワイヤガイドの間に働く力のうち、ワイヤの長手方向の成分のみを考慮し、半径方向の成分を考慮しない。
［７］：ワイヤの伸縮量は、ワイヤに作用する張力に比例する。
［８］：ワイヤの曲げモーメントは、たわみ角に比例する。
［９］：各ワイヤの引っ張り剛性と曲げ剛性とは等しい。
［１０］：各ワイヤのヤング率、断面積、断面２次モーメントは等しい。

遠位ワイヤガイド１６０と各ワイヤに作用する力及びモーメントのつり合いの式から、ａ、ｂ、ｃワイヤの伸縮量Δｌ_ａ、Δｌ_ｂ、Δｌ_ｃを導出する。本実施形態では、湾曲区間の中心軸の延長線上にｚ_２軸をとり、ｚ_１ｗ_１平面上のｚ_２軸と直行する方向にｘ_２軸をとる。そして、ｚ_２，ｘ_２と直行する右手系の座標軸としてｙ_２をとる。上述した仮定４、仮定５及び仮定６から、遠位ワイヤガイドには、ａ、ｂ、ｃワイヤから受ける長手方向の張力ｆ_ａ、ｆ_ｂ、ｆ_ｃと、ワイヤの曲げによるｙ_２軸周りのモーメントＭ_ａ、Ｍ_ｂ、Ｍ_ｃが作用する。遠位ワイヤガイド１６０のＺ_２方向の力のつり合いは、力ｆ_１ａ，ｆ_１ｂ及びｆ_１ｃを用いて、以下の（９）式で表される。
ｆ_ａ＋ｆ_ｂ＋ｆ_ｃ＝０ （９）

また、Ｙ_２軸まわりの曲げモーメントＭ_ａ，Ｍ_ｂ，Ｍ_ｃとワイヤから受ける力ｆ_ａ，ｆ_ｂ，ｆ_１ｃによって生じるモーメントのつり合いは、以下の（１０）式で表される。

さらに、Ｘ_２軸まわりには、力ｆ_ａ，ｆ_ｂ，ｆ_ｃによるモーメントが生じるため、つり合いの式は、以下の（１１）式で表される。

次に、（９）式、（１０）式及び（１１）式を変形して中心軸長さとワイヤ伸縮量の関係を求める。

上述した仮定７、仮定８、仮定９より、力ｆ_ａ，ｆ_ｂ，ｆ_ｃ及びモーメントＭ_ａ，Ｍ_ｂ，Ｍ_ｃは、伸縮量Δｌ_１ａ、Δｌ_１ｂ、Δｌ_１ｃと湾曲角度θ_１を用いて、それぞれ、以下の（１２）式及び（１３）式で表される。

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

ｋ_ｍ＝ＥＩ （１５）

伸縮量Δｌ_１ａ、Δｌ_１ｂ、Δｌ_１ｃが中心軸長さと比べて十分小さいと仮定すると、湾曲区間におけるａ、ｂ、ｃワイヤの長さｌ_１ａ、ｌ_１ｂ、ｌ_１ｃは（１６）式で表すことができる。

（９）式～（１３）式と（１６）式から、力ｆ_ａ，ｆ_ｂ，ｆ_ｃ及びモーメントＭ_ａ，Ｍ_ｂ，Ｍ_ｃを消去し、ベクトルΔｌ_１、ｍ_１、行列Ａ、Ｋ_ｅをそれぞれ（１７）式、（１８）式、（１９）式、（２０）式と定義すると、ワイヤ伸縮量Δｌ_１は（２１）式と表わされる。
Δｌ_１＝［Δｌ_１ａ Δｌ_１ｂ Δｌ_１ｃ］^Ｔ （１７）

Δｌ_１＝（ＡＫ_ｅ）^－１ｍ_１ （２１）

（制御系設計）
図７に、ワイヤの伸縮を補償する本実施形態に係る制御装置３０３のブロック線図を示す。なお、ブロックＫｉｎｅｍａｔｉｃｓとブロックＫは第１の実施形態で説明したものと共通するので、ここでは説明を省略する。ブロックＫ_ｗは、目標角度ベクトルθ_ｒｅｆ１，ζ_ｒｅｆ１及び、中心軸長さｌ_ｄｃを入力として、（２１）式を用いて各ワイヤの伸縮量Δｌ_１ａ、Δｌ_１ｂ、Δｌ_１ｃを演算し、（２２）式で表されるワイヤ伸縮補償量ｌ_ｅ１ａ、ｌ_ｅ１ｂ、ｌ_ｅ１ｃとして出力する。
ｌ_ｅ１ａ＝－Δｌ_１ａ
ｌ_ｅ１ｂ＝－Δｌ_１ｂ （２２）
ｌ_ｅ１ｃ＝－Δｌ_１ｃ

そして、ワイヤ伸縮補償量ｌ_ｅ１ａ、ｌ_ｅ１ｂ、ｌ_ｅ１ｃと差分ｌ_ｓｂ１を、ワイヤ駆動変位ｌ_ｐ１ａ，ｌ_ｐ１ｂ，ｌ_ｐ１ｃにそれぞれ加算する。これにより、ワイヤが伸縮する場合においても、近位最小隙間長さがｌ_ｅ１であり、湾曲、旋回角度をθ_１，ζ_１とするワイヤ駆動変位が求められる。

（シミュレーション）
上記の制御系を用いて、連続体ロボット１００を旋回させた場合のシミュレーション結果を説明する。本実施形態では、連続体ロボット１００の旋回角度ζ_１を０度から１度ステップで３５９度まで変化させるときの、近位最小隙間長さｌ_ｅ１と湾曲角度θ_１を求める。シミュレーションでは、湾曲区間の中心軸初期長さｌ_１ｄを０．０１０ｍ、ワイヤの全長Ｌ_０を１．０ｍとし、近位最小隙間長さｌ_ｅ１が０．００１２５ｍとなるようにワイヤ駆動量を求める。

目標湾曲角度θ_ｒｅｆ１を１２０度とするシミュレーション応答を図８（ａ）、（ｂ）に示す。本実施形態に係る制御系による応答を実線で示す。また、比較として、ワイヤの伸縮を考慮しない第１の実施形態の制御の応答を破線で、（８）式に示す第２の実施形態の制御の応答を点線で、それぞれ示す。

図８（ａ）に湾曲角度θ_１の応答を示す。本実施形態の制御では、旋回角度ζ_１に依らず湾曲角度θ_１が旋回角度ζ_１と一致する。しかし、第１の実施形態の制御では、ワイヤの伸縮により湾曲角度θ_１が目標湾曲角度θ_ｒｅｆ１と比べて小さくなってしまう。また、第２の実施形態の制御では、制御誤差を低減しているが、誤差を完全に補償をすることはできない。

図８（ｂ）に近位最小隙間長さｌ_ｅ１の応答を示す。本実施形態の制御では旋回角度ζ_１に依らず一定である。しかし、第１の実施形態、および第２の実施形態の制御では湾曲角度誤差の影響により隙間長さが増加し、さらに旋回角度ζ_１に応じて長さが変動してしまっている。

上記の結果が示すように、静力学モデルを用いてワイヤの伸縮量を演算する本実施形態の制御系によれば、近位最小長さの変動および湾曲角度誤差を低減可能である。

（第４の実施形態）
先の各実施形態では、１つの湾曲区間を有する連続体ロボットを例に説明した、本実施形態では、複数の湾曲区間を有する連続体ロボットを対象とする。

３．１）モデリング
図９に湾曲区間数ｎの連続体ロボットの概略図と座標系を示す。図９には、ｎ個の湾曲区間のうち連続する３の湾曲区間の部分を抜き出して示している。図１に示した連続体ロボットと同様に、各湾曲区間は、３本のワイヤによりその姿勢が制御される構成となっており、３本のうちの１本のワイヤは、当該湾曲区間の各ワイヤガイドと固定されている。図９に示す構成においては、各湾曲区間のディスタルガイドであるワイヤガイド１６０２、１６０３と、ｘ１ｙ１平面が各湾曲区間に対する基準面となる。

第ｎ湾曲区間の遠位端およびワイヤガイドは、それよりも遠位の湾曲区間のワイヤガイドを兼ねる。本発明では、第ｎ湾曲区間の相対座標系として、第ｎ－１区間の中心軸の延長線上にｚ_ｎ軸をとり、ｚ_ｎ－１ｗ_ｎ－１平面上のｚ_ｎ軸と直行する方向にｘ_ｎ軸をとる。そして、ｚ_ｎ，ｘ_ｎと直行する右手系の座標軸としてｙ_ｎをとる。そして、第ｎ湾曲区間の相対座標系における湾曲角度、旋回角度を

と表記する。

遠位端の湾曲角度および旋回角度を

とするための、相対座標系ｘ_ｎｙ_ｎｚ_ｎにおけるａ，ｂ，ｃワイヤの駆動変位

の候補は、

となる。

第ｎ湾曲区間の近位最小隙間長さｌ_ｅｎは、実施例１と同様に、湾曲角度

が正のとき、

となり、湾曲角度

が負のとき、

となる。

３．２）制御系設計
図１０に、近位最小隙間長さｌ_ｅｎを旋回角度

に依存せず一定とする制御系のブロック線図を示す。目標角度ベクトル

を

と表すと、ブロックＫｉｎｅｍａｔｉｃｓは、式（１６）によりワイヤ駆動変位

を求める。実施例１に示した制御系と同様にブロックＫは近位最小隙間長さｌ_ｅ＝［ｌ_ｅ１ｌ_ｅ２・・・ｌ_ｅｎ］を入力とし、それを対応する相対座標系における旋回角度

に依存せず一定とするための、湾曲区間の中心軸の長さｌ_ｎｄｃ（ｎ＝１，．．．，ｅ）を出力する。中心軸の長さｌ_ｎｄｃ（ｎ＝１，．．．，ｅ）を求めるには、式（１０），（１１）の中心軸初期長さｌ_ｎｄをｌ_ｎｄｃと置き換え、中心軸長さｌ_ｎｄｃについて解けばよい。湾曲角度

が正のとき、

となり、湾曲角度

が負のとき、

となる。そして、中心軸初期長さｌ_ｎｄからの差分に実施例２と同様の調整ゲインを乗じ、隙間長さ補償量ｌ_ｎｄｄ、
ｌ_ｎｄｄ＝ｒｇ_ｎ（ｌ_ｎｄｃ－ｌ_ｎｄ） （１５）
とする。これをブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位、

に加算する。これにより、第ｎの相対座標系における近位最小隙間長さがｌ_ｅｎであり、湾曲、旋回角度をθ_ｎ，ζ_ｎとするワイヤ駆動変位が求められる。

つぎに、アクチュエータの制御入力として必要な第ｎ湾曲区間のワイヤ駆動変位ｌ_ｐｎａ，ｌ_ｐｎｂ，ｌ_ｐｎｃを求める。まず、第ｎの相対座標系におけるワイヤ駆動変位の記号を以下のように定める。

：第ｎ湾曲区間の遠位端に接続されているａ，ｂ，ｃワイヤの第ｍ湾曲区間のおける駆動変位
これは、

となる。これを用いて、ワイヤ駆動変位ｌ_ｐｎａ，ｌ_ｐｎｂ，ｌ_ｐｎｃは、第１～ｎ区間までの相対座標系におけるワイヤの駆動変位の総和となり、

となる。

本実施形態においても、第ｎ番目の湾曲区間の最も近位側のワイヤガイド１６４ｎと、これに隣接する湾曲区間の遠位ワイヤガイド１６０（ｎ＋１）との間の最少隙間長さを一定に保つことができる。この結果、従来技術と比較して連続体ロボットの制御精度を向上させることができる。

（第５の実施形態）
第４の実施形態においては、複数の湾曲区間を有する連続体ロボットに対して湾曲区間毎に相対座標系を導入し、運動学の導出と制御系の設計を行った。しかし、絶対座標系で設定される経路に対して最遠位湾曲区間の角度を追従させる制御では、絶対座標系による制御が簡便となる。そこで、本実施例では、絶対座標系を用いる制御を対象とする。

４．１）モデリング
連続体ロボット１００は、第４の実施形態で説明したものと共通するので、ここでは説明を省略する。

４．２）座標変換
まず、相対角度から絶対座標系における湾曲角度θ_ｎ、旋回角度ζ_ｎを求める手順を示す。第ｎ湾曲区間の相対座標系において各湾曲形状の延長線上に遠位端

から長さ１の点

を取ると、その座標は、

となり、絶対座標系における位置（ｘ_ｔｔｎ，ｙ_ｔｔｎ，ｚ_ｔｔｎ）は、回転変換行列を用いて、

となる。ここで、Ｒ_ｚ（θ），Ｒ_ｙ（θ）はそれぞれｚ軸、ｙ軸まわりの回転行列であり、

である。これを用いて、以下の単位ベクトルｅ_ｚｎを定義する。
ｅ_ｚｎ＝（ｘ_ｔｔｎ－ｘ_ｔｎ，ｙ_ｔｔｎ－ｙ_ｔｎ，ｚ_ｔｔｎ－ｚ_ｔｎ） （２０）

単位ベクトルｅ_ｚｎは第ｎ湾曲区間遠位端の絶対座標系における方向を表し、ｚ軸からの角度が湾曲角度θ_ｎとなり、単位ベクトルｅ_ｚｎのｘｙ平面への射影のｘ軸からの角度が旋回角度ζ_ｎとなる。絶対角度から相対角度を求めるには、単位ベクトルｅ_ｚｎの方向に第ｎ＋１湾曲区間に対する相対座標系の座標軸ｚ_ｎ＋１をとり、第ｎ湾曲区間の相対座標系のｗ_ｎｚ_ｎ平面上においてｚ_ｎ＋１軸と直行する方向にｘ_ｎ＋１軸をとる。右手系より第ｎ＋１湾曲区間の相対座標系が定義されるので、座標軸と単位ベクトルｅ_ｚｎ＋１の関係から相対角度が求められる。第１湾曲区間は相対角度と絶対角度が等しいので、これを近位端から繰り返すことで、全ての湾曲区間の相対角度を求めることができる。

４．３）制御系設計
絶対座標系を用いると、中心軸一定制御においては、アクチュエータの制御入力となる第ｎ湾曲区間のワイヤの駆動変位ｌ_ｐｎａ０，ｌ_ｐｎｂ０，ｌ_ｐｎｃ０を以下のように直接的に求めることができる。

しかし、近位最小隙間長さを旋回角度に依存せず一定とするには、前節の手順により絶対座標系の湾曲、旋回角度θ_ｎ，ζ_ｎから相対座標における湾曲、旋回角度

を求め、それを用いて相対座標系におけるａワイヤ駆動量

を求めたのち、実施例３において式（１３）～（１７）で示した演算を行う必要があり、演算量が増えてしまう。そこで、本実施例では絶対空間において近似値による簡便な手法を示す。

まず、第ｎ湾曲区間の湾曲角度θ_ｎをその相対座標における近似的な湾曲角度

に変換することを考える。隣接する２つの湾曲区間である第ｎ－１湾曲区間と第ｎ湾曲区間において、例えば旋回角度ζ_ｎ－１とζ_ｎが等しい場合、相対座標における近似的な湾曲角度は、

とすればよい。一方、例えば旋回角度ζ_ｎ－１とζ_ｎが９０度の差をもつ場合は、

とすればよい。そこで、旋回角度の差分を引数とする関数αを導入し、相対座標における近似的な湾曲角度を以下のように求めればよい。
α＝ｃｏｓ（（ζ_ｎ－ζ_ｎ－１）ｍｏｄ２π）） （２４）

つぎに、相対座標系におけるａワイヤ駆動量

を座標変換を用いることなく求める。まず、本実施例ではワイヤ駆動変位の記号を以下のように定める。

ｌ_ｐｎａｍ，ｌ_ｐｎｂｍ，ｌ_ｐｎｃｍ：第ｎ湾曲区間の遠位端に接続されているａ，ｂ，ｃワイヤが仮想的に第ｍ湾曲区間を駆動しているとする駆動変位
これは、

となり、これを用いて第ｎ湾曲区間の相対座標系におけるａワイヤ駆動量

は、

となる。これを、実施例３における式（１３）～（１５）に代入すると、隙間長さ補償量ｌ_ｎｄｄが求められる。これを用いて、ワイヤ駆動変位ｌ_ｐｎａ，ｌ_ｐｎｂ，ｌ_ｐｎｃは、中心軸一定制御に用いるワイヤの駆動変位ｌ_ｐｎａ０，ｌ_ｐｎｂ０，ｌ_ｐｎｃ０に、第１～ｎ区間の隙間長さ補償量の総和を加え、

となる。

上記のとおり、絶対座標系を用いても実施形態３と同様の中心軸一定制御を実現できる。本実施形態においても、第ｎ番目の湾曲区間の最も近位側のワイヤガイド１６４ｎと、これに隣接する湾曲区間の遠位ワイヤガイド１６０（ｎ＋１）との間の最少隙間長さを一定に保つことができる。この結果、従来技術と比較して連続体ロボットの制御精度を向上させることができる。

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

このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。

上記で説明した各実施形態は例示的なものにすぎず、本発明の技術的思想から逸脱しない限りにおいて様々に変更あるいは組み合わせて実施することができる。

１００ 連続体ロボット
１１１－１１３ ワイヤ
１６０－１６４ ワイヤガイド
１３０－１３２ アクチュエータ
３００－３０２ 制御装置

Claims (6)

1. 第１ワイヤと、
   第２ワイヤと、
   前記第１ワイヤ及び前記第２ワイヤを保持するディスタルガイドと、
   前記第１ワイヤ及び前記第２ワイヤに対して摺動可能なプロクシマルガイドと、
   前記ディスタルガイドと前記プロクシマルガイドとの間に設けられた複数のワイヤガイドと、
   前記第１ワイヤ及び前記第２ワイヤとを駆動する駆動部と、
   前記駆動部を制御する制御部と、を有し、
   前記第１ワイヤは、前記複数のワイヤガイドに対して固定され、
   前記第２ワイヤは、前記複数のワイヤガイドに対して摺動可能であり、
   前記制御部は、前記複数のワイヤガイドのうち、前記プロクシマルガイドと最も近接して設けられたワイヤガイドと、前記プロクシマルガイドとの間の距離が一定となるように、前記駆動部を制御すること
   を特徴とする連続体ロボット。
2. 基準面を介して延伸する第１および第２のワイヤと、
   前記第１および第２のワイヤが異なる位置に固定された第１のワイヤガイドと、
   前記基準面と前記第１のワイヤガイドとの間に前記第１および第２のワイヤを案内する第２のワイヤガイドと、を有し、前記第１および第２のワイヤの少なくとも一方を駆動することにより湾曲可能な湾曲部を、有する連続体ロボットと、
   前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記第１および第２のワイヤの少なくとも一方の第１の駆動変位量を算出する第１の算出手段と、
   を有することを特徴とする連続体ロボットの制御システムにおいて、
   前記第２のワイヤガイドは、前記第１のワイヤが固定され、
   前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との目標距離に応じて、前記第１および第２のワイヤの少なくとも一方の第２の駆動変位量を算出する第２の算出手段と、
   前記第１の駆動変位量と前記第２の駆動変位量とを加算する加算手段と、
   を有することを特徴とする連続体ロボットの制御システム。
3. 前記第２の駆動変位量にゲインを乗ずるゲイン付与手段をさらに有し、
   前記ゲイン付与手段は、前記第２の駆動変位量に応じて前記ゲインを設定することを特徴とする請求項２に記載の連続体ロボットの制御システム。
4. 前記目標湾曲角度と前記目標旋回角度と前記第２の駆動変位量に応じて前記第１および第２のワイヤの伸縮量（Δｌ_１ａ、Δｌ_１ｂ、Δｌ_１ｃに対応）に基づく補償量（ｌ_ｅ１ａ、ｌ_ｅ１ｂ、ｌ_ｅ１ｃに対応）を演算する第３の算出手段（Ｋ_ｗに対応）と、
   前記第１の駆動変位量と前記第２の駆動変位量と前記補償量とを加算する加算手段と、
   を有することを特徴とする請求項２に記載の連続体ロボットの制御システム。
5. 複数の湾曲区間と、
   前記複数の湾曲区間の各々について、前記第１および第２の算出手段をそれぞれ有すること
   を特徴とする請求項２または３に記載の連続体ロボットの制御システム。
6. 前記第１および第２の算出手段は、一の前記湾曲部の前記第１のワイヤガイドを、隣接する一の前記湾曲部の基準面とする相対座標系を用いて前記第１および第２の駆動変位量を算出すること
   を特徴とする請求項２～５のいずれか１項に記載の連続体ロボットの制御システム。
   
   
   

Images