(57)【要約】【課題】  従来知られていた、ワイヤを駆動することにより形状を制御する連続体ロボットでは、形状制御の精度が不十分となるおそれがあった。【解決手段】  第１ワイヤと、第２ワイヤと、前記第１ワイヤ及び前記第２ワイヤを保持するディスタルガイドと、前記第１ワイヤ及び前記第２ワイヤに対して摺動可能なプロクシマルガイドと、  前記ディスタルガイドと前記プロクシマルガイドとの間に設けられた複数のワイヤガイドと、前記第１ワイヤ及び前記第２ワイヤとを駆動する駆動部と、前記駆動部を制御する制御部と、を有し、前記第１ワイヤは、前記複数のワイヤガイドに対して固定され、前記第２ワイヤは、前記複数のワイヤガイドに対して摺動可能であり、前記制御部は、前記複数のワイヤガイドのうち、前記プロクシマルガイドと最も近接して設けられたワイヤガイドと、前記プロクシマルガイドとの間の距離が一定となるように、前記駆動部を制御する。【選択図】  図３
2019202137.tif 000002
【特許請求の範囲】
【請求項１】
第１ワイヤと、
第２ワイヤと、
前記第１ワイヤ及び前記第２ワイヤを保持するディスタルガイドと、
前記第１ワイヤ及び前記第２ワイヤに対して摺動可能なプロクシマルガイドと、
前記ディスタルガイドと前記プロクシマルガイドとの間に設けられた複数のワイヤガイドと、
前記第１ワイヤ及び前記第２ワイヤとを駆動する駆動部と、
前記駆動部を制御する制御部と、を有し、
前記第１ワイヤは、前記複数のワイヤガイドに対して固定され、
前記第２ワイヤは、前記複数のワイヤガイドに対して摺動可能であり、
前記制御部は、前記複数のワイヤガイドのうち、前記プロクシマルガイドと最も近接して設けられたワイヤガイドと、前記プロクシマルガイドとの間の距離が一定となるように、前記駆動部を制御すること
を特徴とする連続体ロボット。
【請求項２】
基準面を介して延伸する第１および第２のワイヤと、
前記第１および第２のワイヤが異なる位置に固定された第１のワイヤガイドと、
前記基準面と前記第１のワイヤガイドとの間に前記第１および第２のワイヤを案内する第２のワイヤガイドと、を有し、前記第１および第２のワイヤの少なくとも一方を駆動することにより湾曲可能な湾曲部を、有する連続体ロボットと、
前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記第１および第２のワイヤの少なくとも一方の第１の駆動変位量を算出する第１の算出手段と、
を有することを特徴とする連続体ロボットの制御システムにおいて、
前記第２のワイヤガイドは、前記第１のワイヤが固定され、
前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との目標距離に応じて、前記第１および第２のワイヤの少なくとも一方の第２の駆動変位量を算出する第２の算出手段と、
前記第１の駆動変位量と前記第２の駆動変位量とを加算する加算手段と、
を有することを特徴とする連続体ロボットの制御システム。
【請求項３】
前記第２の駆動変位量にゲインを乗ずるゲイン付与手段をさらに有し、
前記ゲイン付与手段は、前記第２の駆動変位量に応じて前記ゲインを設定することを特徴とする請求項２に記載の連続体ロボットの制御システム。
【請求項４】
前記目標湾曲角度と前記目標旋回角度と前記第２の駆動変位量に応じて前記第１および第２のワイヤの伸縮量（Δｌ１ａ、Δｌ１ｂ、Δｌ１ｃに対応）に基づく補償量（ｌｅ１ａ、ｌｅ１ｂ、ｌｅ１ｃに対応）を演算する第３の算出手段（Ｋｗに対応）と、
前記第１の駆動変位量と前記第２の駆動変位量と前記補償量とを加算する加算手段と、
を有することを特徴とする請求項２に記載の連続体ロボットの制御システム。
【請求項５】
複数の前記湾曲区間と、
前記複数の湾曲区間の各々について、前記第１および第２の算出手段をそれぞれ有すること
を特徴とする請求項２または３に記載の連続体ロボットの制御システム。
【請求項６】
前記第１および第２の算出手段は、一の前記湾曲部の前記第１のワイヤガイドを、隣接する一の前記湾曲部の基準面とする相対座標系を用いて前記第１および第２の駆動変位量を算出すること
を特徴とする請求項２〜５のいずれか１項に記載の連続体ロボットの制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、連続体ロボットおよび連続体ロボットの制御システムに関する。
【背景技術】
【０００２】
連続体ロボットはコンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間からなり、その湾曲区間を変形させることにより形状が制御される。このロボットは、剛体リンクにより構成されるロボットに対して、主に二つの優位性を持つ。一つ目は、剛体リンクロボットがはまり込んでしまうような、狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。二つ目は、連続体ロボットは本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく対象物内を移動することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、内視鏡のシースやカテーテルなど医療の分野や、レスキューロボットなど極限作業ロボットへの応用が期待されている。連続体ロボットの駆動方法として、腱駆動によるもの、押引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。非特許文献１には、３本のワイヤと、それをガイドするスペーサーディスクと呼ばれるワイヤガイド、エンドディスクと呼ばれる遠位端、および基台部により構成される連続体ロボットが開示されている。運動学を導出することでワイヤの押引き駆動量を求め、連続体ロボットの形状の制御を行っている。非特許文献１に記載されたロボットでは、それぞれのワイヤガイドの円周に沿ってワイヤが挿通される孔が設けられている。ワイヤは遠位端であるエンドディスクにのみ固定され、基台部とエンドディスクとの間に設けられたワイヤガイドに対して摺動する構造である。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】Ｋ．Ｘｕ，Ｍ．Ｆｕ，ａｎｄ  Ｊ．Ｚｈａｏ，”Ａｎ  Ｅｘｐｅｒｉｍｅｎｔａｌ  Ｋｉｎｅｓｔａｔｉｃ  Ｃｏｍｐａｒｉｓｏｎ  ｂｅｔｗｅｅｎ  Ｃｏｎｔｉｎｕｕｍ  Ｍａｎｉｐｕｌａｔｏｒｓ  ｗｉｔｈ  Ｓｓｐａｎｕｃｔｕｒａｌ  Ｖａｒｉａｔｉｏｎｓ，”ｉｎ  ＩＥＥＥ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ  （ＩＣＲＡ），Ｈｏｎｇ  Ｋｏｎｇ，Ｃｈｉｎａ，２０１４，ｐｐ．３２５８−３２６４．
【発明の概要】
【発明が解決しようとする課題】
【０００４】
非特許文献１において、連続体ロボットの運動を表現するための運動学の導出においては、ワイヤは一様な曲率で変形することを仮定し、さらに、連続体ロボットの中心を通る仮想的なワイヤの長さが、連続体ロボットの形状に依らず常に一定となるようにワイヤの駆動量を求めている。しかし、ワイヤの駆動によるワイヤとワイヤガイド等との間の摺動摩擦によりワイヤガイドの位置が変動すると、ワイヤ曲率が非一様となり、それを一様と仮定する運動学との誤差が大きくなる。そのため、形状制御の精度が不十分となるおそれがある。
【０００５】
上記課題に鑑みて、本発明は、連続体ロボットの制御の精度を向上させることを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決する本発明の一の側面である連続体ロボットは、第１ワイヤと、第２ワイヤと、前記第１ワイヤ及び前記第２ワイヤを保持するディスタルガイドと、前記第１ワイヤ及び前記第２ワイヤに対して摺動可能なプロクシマルガイドと、前記ディスタルガイドと前記プロクシマルガイドとの間に設けられた複数のワイヤガイドと、前記第１ワイヤ及び前記第２ワイヤとを駆動する駆動部と、前記駆動部を制御する制御部と、を有し、前記第１ワイヤは、前記複数のワイヤガイドに対して固定され、前記第２ワイヤは、前記複数のワイヤガイドに対して摺動可能であり、前記制御部は、前記複数のワイヤガイドのうち、前記プロクシマルガイドと最も近接して設けられたワイヤガイドと、前記プロクシマルガイドとの間の距離が一定となるように、前記駆動部を制御することを特徴とする。
【０００７】
本発明の別の一の側面である連続体ロボットの制御システムは、基準面を介して延伸する第１および第２のワイヤと、前記第１および第２のワイヤが異なる位置に固定された第１のワイヤガイドと、前記基準面と前記第１のワイヤガイドとの間に前記第１および第２のワイヤを案内する第２のワイヤガイドと、を有し、前記第１および第２のワイヤの少なくとも一方を駆動することにより湾曲可能な湾曲部を、有する連続体ロボットと、前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記第１および第２のワイヤの少なくとも一方の第１の駆動変位量を算出する第１の算出手段と、を有することを特徴とする連続体ロボットの制御システムにおいて、前記第２のワイヤガイドは、前記第１のワイヤが固定され、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との目標距離に応じて、前記第１および第２のワイヤの少なくとも一方の第２の駆動変位量を算出する第２の算出手段と、前記第１の駆動変位量と前記第２の駆動変位量とを加算する加算手段と、を有することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、連続体ロボットの制御精度を向上させることが可能となる。
【図面の簡単な説明】
【０００９】
【図１】実施形態１に係る連続体ロボットの運動学モデルを示す図である。
【図２】実施形態１に係る連続体ロボットの運動学モデルを説明するための図である。
【図３】実施形態１に係る連続体ロボットの制御システムを示すブロック線図である。
【図４】実施形態１に係るシミュレーション応答を示す図である。
【図５】実施形態１に係るシミュレーション応答を示す別の図である。
【図６】実施形態２に係る連続体ロボットの制御システムを示すブロック線図である。
【図７】実施形態３に係る連続体ロボットの制御システムを示すブロック線図である。
【図８】実施形態３に係るシミュレーション応答を示す図である。
【図９】実施形態４に係る連続体ロボットの運動学モデルを示す図である。
【図１０】実施形態４に係る連続体ロボットの制御システムを示すブロック線図である。
【発明を実施するための形態】
【００１０】
以下に、本発明の各実施形態について、図面を参照して詳細に説明する。各実施形態では、連続体ロボット（連続体マニピュレータともいう）の制御システムが可撓性内視鏡に適用される例を示す。なお、本発明の実施形態に係る連続体ロボットの制御システムの例として適用する可撓性内視鏡は、医療分野に限定されるものではなく、湾曲部を挿入・抜去させる経路（以下、「挿抜経路」を称する）の内部を観察するものであれば他の分野（例えば、配管等の内部を観察するための工業用内視鏡）にも適用可能である。
【００１１】
（第１の実施形態）
本実施形態では、３本のワイヤを駆動することで立体的に形状の制御が可能な連続体ロボットの運動学を導出し、最も近位にあるワイヤガイドと基台部の距離を一定とするための制御系を示す。
【００１２】
以下に、連続体ロボットの運動学の導出と制御アルゴリズムについて詳細に記述する。
【００１３】
１．１）モデリング
図１に本実施例で用いる連続体ロボット１００の概略図を示す。ここでは、連続体ロボット１００が１つの湾曲区間を有する例を示す。連続体ロボット１００の湾曲区間の遠位端に設けられた遠位ワイヤガイド（以下、ディスタルガイドとも称す）１６０とワイヤ１１１〜１１３とは、接続部１２１〜１２３においてそれぞれ接続される。また、連続体ロボット１００は、ワイヤ１１１〜１１３を案内するための部材であるワイヤガイド１６１〜１６４を有する。最も基端側（近位側）に設けられたワイヤガイド１６４を、近位ワイヤガイドあるいはプロクシマルガイドとも称する。ワイヤガイド１６０〜１６４は、円盤状の部材として図示しているが、この例に限らず、たとえば円環状の部材としてもよい。破線で示される連続体ロボット１００の中心軸を含む領域に開口を持つワイヤガイドとすることにより、この開口部にカメラなどのツールを導入することができる。本例のように、複数のワイヤガイドを離散的に配置する方法のほかに、蛇腹状やメッシュ状などの連続体部材をワイヤガイドとして用いても良い。ワイヤガイド１６１〜１６４はワイヤ１１１に、固定部１５０〜１５３において固定される。一方、ワイヤ１１２、１１３は、ワイヤガイド１６１〜１６４に対して摺動可能に構成されている。また、本明細書では、各ワイヤガイドに固定されたワイヤ１１１をａワイヤとよび、ワイヤ１１２，１１３をｘｙ面内においてａワイヤから反時計回りにそれぞれｂワイヤ、ｃワイヤと呼ぶ。ａワイヤは第１ワイヤであり、ｂワイヤおよびｃワイヤは第２ワイヤである。各ワイヤ１１１〜１１３のｘｙｚ空間における長さをそれぞれ、ｌ１ａ，ｌ１ｂ，ｌ１ｃとする。また、湾曲区間を駆動するワイヤの駆動変位をｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃとする。ワイヤは図２に示すように、一辺の長さをｒｓとする正三角形の頂点に配置されており、位相角ξ１は第１湾曲区間を駆動するワイヤ配置を決定する角度である。本実施例では、ξ１＝０とする。なお、図２は、任意のワイヤガイド１６１〜１６４をｘｙ面に載置した際に、連続体ロボット１００の遠位端側から近位端に向かって当該ワイヤガイドを見た平面図である。
【００１４】
ワイヤ１１１〜１１３をロボット基台部１４０に設けられた駆動部であるアクチュエータ１３０〜１３２で押引き駆動することにより、湾曲区間、すなわち、連続体ロボット１００の姿勢が制御される。ロボット基台部１４０には、ワイヤ１１１〜１１３が挿通される開口を有しており、ロボット基台部１４０がワイヤガイドを有するということもできる。このロボット基台部１４０の面を基準面として考えることができる。
【００１５】
記号の定義を以下に示す。ｌ１ｄ：湾曲区間の中心軸の初期長さ（連続体ロボットが湾曲していない場合の中心軸の長さ）、θ１：遠位端の湾曲角度（図中ｚ軸に対して遠位ワイヤガイドの法線がなす角度）、ζ１：遠位端の旋回角度、ρ１：湾曲区間の曲率半径、ｒ：ワイヤガイドの半径、ｌ１ａ：ｘｙｚ空間におけるａワイヤの長さ、ｌａｗ１：ａワイヤ先端から最基端側ワイヤガイドの基端側面までのａワイヤ上の距離、ｌｗ１：最基端側ワイヤガイドの基端側面からｘｙ面までのａワイヤ上の距離、ｌｅ１：最基端側ワイヤガイドの基端側面からｘｙ面までの、ｗｚ面内の点Ｃを中心とする半径ρ１−ｒである円弧上の距離（本発明では、これを近位最小隙間長さとよぶ）、ｌｅｏ１：最基端側ワイヤガイドの基端側面からｘｙ面までの、ｗｚ面内の点Ｃを中心とする半径ρ１＋ｒである円弧上の距離（本発明では、これを近位最大隙間長さとよぶ）。
【００１６】
以下のように仮定をおき、連続体ロボットの運動学を導出する。
［１］湾曲区間において、ワイヤは曲率一定に変形する。
［２］ワイヤのねじり変形は考慮しない。
［３］ワイヤは長手方向に変形しない。
［４］ワイヤガイドとワイヤ間の摩擦は考慮しない。
【００１７】
遠位端の湾曲角度および旋回角度をθ１，ζ１とするための、第１湾曲区間のａ，ｂ，ｃワイヤの駆動変位ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃの候補は、
【００１８】
【数１】
2019202137.tif 000003
となる。ここで、例えば図１に示すように、ワイヤガイド１６０〜１６４を等間隔に配置すると、ワイヤガイドはａワイヤに固定されているため、旋回角度ζ１＝０のときａワイヤの負方向の最大駆動変位は、ｔｗｇをワイヤガイド厚さとすると、
−（ｌ１ｄ−５ｔｗｇ）／５
以下に制約される。また、旋回角度ζ１に対して、湾曲の最大角度が変動する。さらに、旋回角度ζ１の変化に伴って基端から基端側のワイヤガイド１６４までの距離が変動する。そのため、連続体ロボットの湾曲区間を被覆するような、例えば樹脂チューブなどを保護構造として連続体ロボット１００に装備した場合、基端から基端側のワイヤガイドまでの保護構造の伸縮量も旋回角度に旋回角度ζ１に対して変動する。そのため、基端から基端側のワイヤガイド１６４までの距離の変動に対応できるよう、保護構造に大きな伸縮性が求められる。この結果、保護構造として採用できる材料の選択肢が狭まることが懸念される。
【００１９】
そこで、本実施例では、近位最小隙間長さｌｅ１が旋回角度ζ１に依存せず一定となる制御を行う。この長さは湾曲角度θ１が正のとき、
【００２０】
【数２】
2019202137.tif 000004
となり、湾曲角度θ１が負のとき、
【００２１】
【数３】
2019202137.tif 000005
となる。
【００２２】
１．２）制御系設計
次に、連続体ロボット１００を制御するための制御系について説明する。ここでは、連続体ロボットに対する目標形状、すなわち目標姿勢から、各ワイヤに対する駆動変位を求めると同時に、ワイヤガイドと基台部の距離を一定とするための駆動変位を求める。そして、それを加算するアルゴリズムを用いる。これにより、旋回角度ζ１に依存せずワイヤガイドと基台部の距離である近位最小隙間長さｌｅ１を一定とすることができる。
【００２３】
図３に、本実施形態に係る連続体ロボット１００の制御部である制御装置３００のブロック線図を示す。ここで、Ｐは連続体ロボット１００を表す。制御装置３００は、ブロックＫおよびブロックＫｉｎｅｍａｔｉｃｓを含んで構成される。Ｋｉｎｅｍａｔｉｃｓは運動学モデルに基づいてワイヤ駆動変位を演算するブロックであり、第１の算出手段に対応する。ブロックＫは第２の算出手段に対応する。入力装置４００は、制御装置３００に対して、近位最少隙間長さｌｅ１、目標角度ベクトルθｒｅｆ１，ζｒｅｆ１を入力する。
【００２４】
目標角度ベクトルθｒｅｆ１，ζｒｅｆ１が設定されると、ブロックＫｉｎｅｍａｔｉｃｓは、ワイヤ駆動変位ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃを求める。ここで、ブロックＫは、近位最小隙間長さｌｅ１、目標角度ベクトルθｒｅｆ１，ζｒｅｆ１を入力として、ｌｅ１が旋回角度ζ１に依存せず一定となるための湾曲区間の中心軸の長さｌｄｃを出力する。そして、中心軸初期長さｌ１ｄからの差分を隙間長さ補償量ｌ１ｄｄとして、ブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃにそれぞれ加算する。これにより、近位最小隙間長さがｌｅ１であり、湾曲、旋回角度をθ１，ζ１とするワイヤ駆動変位が求められる。
【００２５】
ここで、ブロックＫが中心軸の長さｌｄｃを求めるには、式（２），（３）の中心軸初期長さｌ１ｄをｌ１ｄｃと置き換え、中心軸長さｌ１ｄｃについて解けばよい。湾曲角度θ１が正のとき、
【００２６】
【数４】
2019202137.tif 000006
となり、湾曲角度θ１が負のとき、
【００２７】
【数５】
2019202137.tif 000007
となる。そして、中心軸初期長さｌ１ｄからの差分を隙間長さ補償量
ｌ１ｄｄ、ｌ１ｄｄ＝ｌ１ｄｃ−ｌ１ｄ      （６）
とする。そして、隙間長さ補償量ｌ１ｄｄをブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位、ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃに加算する。これにより、近位最小隙間長さがｌｅ１であり、湾曲、旋回角度をθ１，ζ１とするワイヤ駆動変位が求められる。
【００２８】
１．３）シミュレーション
上記の制御系を用いて、連続体ロボット１００を旋回させた場合のシミュレーション結果を説明する。ここでは、連続体ロボット１００の旋回角度ζ１を０度から１度ステップで３５９度まで変化させ、近位最小隙間長さｌｅ１、近位最大隙間長さｌｅｏ１、さらにはワイヤガイド間の最小、最大隙間長さｌｇｉ，ｌｇｏを求める。ワイヤガイド間の隙間長さは近位隙間長さと同様に、ｗｚ面内の点Ｃを中心とする円弧を用いて求めている。シミュレーションでは、湾曲区間の中心軸初期長さｌ１ｄを０．０１０ｍとし、ワイヤガイド数を５、ワイヤガイドの厚みを０．０００７５ｍ、ワイヤガイド間の距離を０．００１２５ｍとして均等配置し、近位最小隙間長さｌｅ１が０．００１２５ｍとなるようにワイヤ駆動量を求める。
【００２９】
湾曲角度θ１を５５度とするシミュレーション応答を図４（ａ）〜（ｃ）に示す。本実施形態に係る制御系（以下、近位最小隙間一定制御と略す）による応答を実線で示し、比較として式（１）のみを用いた中心軸長さを一定とする制御（以下、中心軸長一定制御と略す）の応答を破線で示す。
【００３０】
図４（ａ）に近位最小隙間長さｌｅ１の応答を示す。近位最小隙間一定制御では旋回角度ζ１に依らず一定である。しかし、中心軸長一定制御では旋回角度０度の方向で、ワイヤガイド１６４がａワイヤとともに基台部方向に引き込まれるため、近位最小隙間長さが短くなる。そして、旋回角度１８０度の方向では、近位最小隙間長さが長くなっていることがわかる。つまり、中心軸一定制御では、旋回角度ζ１に応じて近位最少隙間長さが変動する。
【００３１】
図４（ｂ）に、近位最大隙間長さｌｅｏ１の応答を示す。近位最小隙間一定制御では旋回角度ζ１に依らずほぼ一定である。しかし、中心軸長一定制御では近位最小隙間長さと同様の応答を示す。
【００３２】
さらに、図４（ｃ）にワイヤガイド間の最小隙間長さｌｇｉの応答を細線で、最大隙間長さｌｇｏの応答を太線で示す。近位最小隙間一定制御によるワイヤガイド間の最小隙間長さの変動幅は中心軸長一定制御による変動幅よりも大きいが、最大隙間長さの変動幅は中心軸長一定制御による変動幅よりも小さいことがわかる。
【００３３】
上記の結果が示すように、本実施形態で提案する近位最小隙間一定制御によれば、中心軸長一定制御とワイヤガイド間の隙間長さの変動幅は同等でありながら、近位最小長さおよび最大隙間長さの旋回角度による変動幅を低減可能であることがわかる。
【００３４】
つぎに、湾曲角度θ１を９０度とするシミュレーション応答を図５（ａ）〜（ｃ）に示す。中心軸初期長さｌｄ１を０．０１０ｍとすると、湾曲角度θ１が９０度の場合には、中心軸一定制御では近位最小隙間長さが負になってしまい、湾曲区間の湾曲角度の限界を超えるため、中心軸初期長さｌｄ１を０．０１０８ｍとして計算している。図５（ａ）〜（ｃ）の結果から分かるように、本条件下においても図４と同様の傾向を示す。このように、近位最小隙間一定制御によれば、中心軸長一定制御とワイヤガイド間の隙間長さの変動幅は同等でありながら、近位最小長さおよび最大隙間長さの旋回角度による変動幅を低減可能である。
【００３５】
本実施形態によれば、連続体ロボットの制御精度を向上させることができる。
【００３６】
（第２の実施形態）
第１の実施形態では、近位最小隙間長さｌｅ１を旋回角度ζ１に依存せず一定とする制御系を示した。運動学ではワイヤは長手方向に変形しないと仮定しているが、実際は、形状制御によりワイヤが伸縮し、隙間長さ補償の性能が低下する。特に、ワイヤ圧縮時に隙間長さ補償量が負であると、近位端のワイヤガイドが基台部に接触する恐れがある。本実施形態では、このワイヤの伸縮に対して制御系に調整ゲインを導入する。
【００３７】
２．１）モデリング
連続体ロボット１００は、第１の実施形態で説明したものと共通するので、ここでは説明を省略する。
【００３８】
２．２）制御系設計
図６に、本実施形態に係る制御装置３０１のブロック線図を示す。制御装置３０１は、中心軸長さｌ１ｄｃと中心軸初期長さｌ１ｄの差分ｌｓｂ１に調整ゲインｒｇ１を乗じるブロックｒｇ１を備える点で制御装置３００とは異なっている。ブロックｒｇ１により、隙間長さ補償量ｌ１ｄｄは、
ｌ１ｄｄ＝ｒｇ１（ｌ１ｄｃ−ｌ１ｄ）＝ｒｇ１ｌｓｂ１      （７）
となる。ここで、調整ゲインｒｇ１を差分ｌｓｂ１に応じて変動させても良い。例えば、差分ｌｓｂ１正負によって以下のように切り替えてもよい。
【００３９】
【数６】
2019202137.tif 000008
【００４０】
これにより実際のワイヤ長の伸縮に対して制御系を調整可能となる。
【００４１】
（第３の実施形態）
第２の実施形態では、ワイヤが伸縮するときに隙間長さの誤差を低減する方法を示した。しかし、連続体ロボットにおけるワイヤの伸縮量は旋回角度ζ１に依存して非線形に変化するため、（８）式に示すように差分ｌｓｂ１の正負に応じて調整ゲインを切り替える方法では隙間長さを高精度に補償することはできない。そこで、本実施形態では連続体ロボットの静力学モデルを導出し、このモデルを用いてワイヤ伸縮量を演算する。
【００４２】
（モデリング）
本実施形態の説明に用いる記号の定義を以下に示す。
Ｅ：ワイヤのヤング率
Ａ：ワイヤの断面積
Ｉ：ワイヤの断面２次モーメント
Ｌ０：ａ、ｂ、ｃワイヤの全長
なお、本実施形態において、ワイヤ駆動量ｌｐ１ｂ、ワイヤ伸縮量Δｌａ，Δｌｂ，Δｌｃ，ワイヤ張力ｆａ，ｆｂ，ｆｃは、遠位端方向を正とし、また、モーメントＭａ，Ｍｂ，Ｍｃは、時計方向を正とする。
【００４３】
本実施形態では、ワイヤの伸縮を考慮し、第１の実施形態で示した仮定１、仮定２、および仮定４に加えて、以下に記載する仮定５〜仮定１０を前提とした静力学モデルを考える。
［５］：ワイヤガイドとワイヤの間に働く力とモーメントを考慮しない。
［６］：ワイヤと遠位ワイヤガイドの間に働く力のうち、ワイヤの長手方向の成分のみを考慮し、半径方向の成分を考慮しない。
［７］：ワイヤの伸縮量は、ワイヤに作用する張力に比例する。
［８］：ワイヤの曲げモーメントは、たわみ角に比例する。
［９］：各ワイヤの引っ張り剛性と曲げ剛性とは等しい。
［１０］：各ワイヤのヤング率、断面積、断面２次モーメントは等しい。
【００４４】
遠位ワイヤガイド１６０と各ワイヤに作用する力及びモーメントのつり合いの式から、ａ、ｂ、ｃワイヤの伸縮量Δｌａ、Δｌｂ、Δｌｃを導出する。本実施形態では、湾曲区間の中心軸の延長線上にｚ２軸をとり、ｚ１ｗ１平面上のｚ２軸と直行する方向にｘ２軸をとる。そして、ｚ２，ｘ２と直行する右手系の座標軸としてｙ２をとる。上述した仮定４、仮定５及び仮定６から、遠位ワイヤガイドには、ａ、ｂ、ｃワイヤから受ける長手方向の張力ｆａ、ｆｂ、ｆｃと、ワイヤの曲げによるｙ２軸周りのモーメントＭａ、Ｍｂ、Ｍｃが作用する。遠位ワイヤガイド１６０のＺ２方向の力のつり合いは、力ｆ１ａ，ｆ１ｂ及びｆ１ｃを用いて、以下の（９）式で表される。
ｆａ＋ｆｂ＋ｆｃ＝０      （９）
【００４５】
また、Ｙ２軸まわりの曲げモーメントＭａ，Ｍｂ，Ｍｃとワイヤから受ける力ｆａ，ｆｂ，ｆ１ｃによって生じるモーメントのつり合いは、以下の（１０）式で表される。
【００４６】
【数７】
2019202137.tif 000009
【００４７】
さらに、Ｘ２軸まわりには、力ｆａ，ｆｂ，ｆｃによるモーメントが生じるため、つり合いの式は、以下の（１１）式で表される。
【００４８】
【数８】
2019202137.tif 000010
【００４９】
次に、（９）式、（１０）式及び（１１）式を変形して中心軸長さとワイヤ伸縮量の関係を求める。
【００５０】
上述した仮定７、仮定８、仮定９より、力ｆａ，ｆｂ，ｆｃ及びモーメントＭａ，Ｍｂ，Ｍｃは、伸縮量Δｌ１ａ、Δｌ１ｂ、Δｌ１ｃと湾曲角度θ１を用いて、それぞれ、以下の（１２）式及び（１３）式で表される。
【００５１】
【数９】
2019202137.tif 000011
【００５２】
【数１０】
2019202137.tif 000012
【００５３】
なお、（１２）式及び（１３）式において、定数ｋｅと定数ｋｍは、それぞれ、ワイヤの引っ張り剛性と曲げ剛性を表す。そして、この定数ｋｅ及び定数ｋｍは、上述した仮定１０から、ワイヤの全長Ｌ０、ワイヤの断面積Ａ、断面２次モーメントＩ、ヤング率Ｅを用いると、それぞれ、以下の（１４）式及び（１５）式で表すことができる。
【００５４】
【数１１】
2019202137.tif 000013
ｋｍ＝ＥＩ      （１５）
【００５５】
伸縮量Δｌ１ａ、Δｌ１ｂ、Δｌ１ｃが中心軸長さと比べて十分小さいと仮定すると、湾曲区間におけるａ、ｂ、ｃワイヤの長さｌ１ａ、ｌ１ｂ、ｌ１ｃは（１６）式で表すことができる。
【００５６】
【数１２】
2019202137.tif 000014
【００５７】
（９）式〜（１３）式と（１６）式から、力ｆａ，ｆｂ，ｆｃ及びモーメントＭａ，Ｍｂ，Ｍｃを消去し、ベクトルΔｌ１、ｍ１、行列Ａ、Ｋｅをそれぞれ（１７）式、（１８）式、（１９）式、（２０）式と定義すると、ワイヤ伸縮量Δｌ１は（２１）式と表わされる。
Δｌ１＝［Δｌ１ａ  Δｌ１ｂ  Δｌ１ｃ］Ｔ      （１７）
【００５８】
【数１３】
2019202137.tif 000015
【００５９】
【数１４】
2019202137.tif 000016
【００６０】
【数１５】
2019202137.tif 000017
Δｌ１＝（ＡＫｅ）−１ｍ１      （２１）
【００６１】
（制御系設計）
図７に、ワイヤの伸縮を補償する本実施形態に係る制御装置３０３のブロック線図を示す。なお、ブロックＫｉｎｅｍａｔｉｃｓとブロックＫは第１の実施形態で説明したものと共通するので、ここでは説明を省略する。ブロックＫｗは、目標角度ベクトルθｒｅｆ１，ζｒｅｆ１及び、中心軸長さｌｄｃを入力として、（２１）式を用いて各ワイヤの伸縮量Δｌ１ａ、Δｌ１ｂ、Δｌ１ｃを演算し、（２２）式で表されるワイヤ伸縮補償量ｌｅ１ａ、ｌｅ１ｂ、ｌｅ１ｃとして出力する。
ｌｅ１ａ＝−Δｌ１ａ
ｌｅ１ｂ＝−Δｌ１ｂ      （２２）
ｌｅ１ｃ＝−Δｌ１ｃ
【００６２】
そして、ワイヤ伸縮補償量ｌｅ１ａ、ｌｅ１ｂ、ｌｅ１ｃと差分ｌｓｂ１を、ワイヤ駆動変位ｌｐ１ａ，ｌｐ１ｂ，ｌｐ１ｃにそれぞれ加算する。これにより、ワイヤが伸縮する場合においても、近位最小隙間長さがｌｅ１であり、湾曲、旋回角度をθ１，ζ１とするワイヤ駆動変位が求められる。
【００６３】
（シミュレーション）
上記の制御系を用いて、連続体ロボット１００を旋回させた場合のシミュレーション結果を説明する。本実施形態では、連続体ロボット１００の旋回角度ζ１を０度から１度ステップで３５９度まで変化させるときの、近位最小隙間長さｌｅ１と湾曲角度θ１を求める。シミュレーションでは、湾曲区間の中心軸初期長さｌ１ｄを０．０１０ｍ、ワイヤの全長Ｌ０を１．０ｍとし、近位最小隙間長さｌｅ１が０．００１２５ｍとなるようにワイヤ駆動量を求める。
【００６４】
目標湾曲角度θｒｅｆ１を１２０度とするシミュレーション応答を図８（ａ）、（ｂ）に示す。本実施形態に係る制御系による応答を実線で示す。また、比較として、ワイヤの伸縮を考慮しない第１の実施形態の制御の応答を破線で、（８）式に示す第２の実施形態の制御の応答を点線で、それぞれ示す。
【００６５】
図８（ａ）に湾曲角度θ１の応答を示す。本実施形態の制御では、旋回角度ζ１に依らず湾曲角度θ１が旋回角度ζ１と一致する。しかし、第１の実施形態の制御では、ワイヤの伸縮により湾曲角度θ１が目標湾曲角度θｒｅｆ１と比べて小さくなってしまう。また、第２の実施形態の制御では、制御誤差を低減しているが、誤差を完全に補償をすることはできない。
【００６６】
図８（ｂ）に近位最小隙間長さｌｅ１の応答を示す。本実施形態の制御では旋回角度ζ１に依らず一定である。しかし、第１の実施形態、および第２の実施形態の制御では湾曲角度誤差の影響により隙間長さが増加し、さらに旋回角度ζ１に応じて長さが変動してしまっている。
【００６７】
上記の結果が示すように、静力学モデルを用いてワイヤの伸縮量を演算する本実施形態の制御系によれば、近位最小長さの変動および湾曲角度誤差を低減可能である。
【００６８】
（第４の実施形態）
先の各実施形態では、１つの湾曲区間を有する連続体ロボットを例に説明した、本実施形態では、複数の湾曲区間を有する連続体ロボットを対象とする。
【００６９】
３．１）モデリング
図９に湾曲区間数ｎの連続体ロボットの概略図と座標系を示す。図９には、ｎ個の湾曲区間のうち連続する３の湾曲区間の部分を抜き出して示している。図１に示した連続体ロボットと同様に、各湾曲区間は、３本のワイヤによりその姿勢が制御される構成となっており、３本のうちの１本のワイヤは、当該湾曲区間の各ワイヤガイドと固定されている。図９に示す構成においては、各湾曲区間のディスタルガイドであるワイヤガイド１６０２、１６０３と、ｘ１ｙ１平面が各湾曲区間に対する基準面となる。
【００７０】
第ｎ湾曲区間の遠位端およびワイヤガイドは、それよりも遠位の湾曲区間のワイヤガイドを兼ねる。本発明では、第ｎ湾曲区間の相対座標系として、第ｎ−１区間の中心軸の延長線上にｚｎ軸をとり、ｚｎ−１ｗｎ−１平面上のｚｎ軸と直行する方向にｘｎ軸をとる。そして、ｚｎ，ｘｎと直行する右手系の座標軸としてｙｎをとる。そして、第ｎ湾曲区間の相対座標系における湾曲角度、旋回角度を
【００７１】
【数１６】
2019202137.tif 000018
と表記する。
【００７２】
遠位端の湾曲角度および旋回角度を
【００７３】
【数１７】
2019202137.tif 000019
とするための、相対座標系ｘｎｙｎｚｎにおけるａ，ｂ，ｃワイヤの駆動変位
【００７４】
【数１８】
2019202137.tif 000020
の候補は、
【００７５】
【数１９】
2019202137.tif 000021
となる。
【００７６】
第ｎ湾曲区間の近位最小隙間長さｌｅｎは、実施例１と同様に、湾曲角度
【００７７】
【数２０】
2019202137.tif 000022
が正のとき、
【００７８】
【数２１】
2019202137.tif 000023
となり、湾曲角度
【００７９】
【数２２】
2019202137.tif 000024
が負のとき、
【００８０】
【数２３】
2019202137.tif 000025
となる。
【００８１】
３．２）制御系設計
図１０に、近位最小隙間長さｌｅｎを旋回角度
【００８２】
【数２４】
2019202137.tif 000026
に依存せず一定とする制御系のブロック線図を示す。目標角度ベクトル
【００８３】
【数２５】
2019202137.tif 000027
を
【００８４】
【数２６】
2019202137.tif 000028
と表すと、ブロックＫｉｎｅｍａｔｉｃｓは、式（１６）によりワイヤ駆動変位
【００８５】
【数２７】
2019202137.tif 000029
を求める。実施例１に示した制御系と同様にブロックＫは近位最小隙間長さｌｅ＝［ｌｅ１ｌｅ２・・・ｌｅｎ］を入力とし、それを対応する相対座標系における旋回角度
【００８６】
【数２８】
2019202137.tif 000030
に依存せず一定とするための、湾曲区間の中心軸の長さｌｎｄｃ（ｎ＝１，．．．，ｅ）を出力する。中心軸の長さｌｎｄｃ（ｎ＝１，．．．，ｅ）を求めるには、式（１０），（１１）の中心軸初期長さｌｎｄをｌｎｄｃと置き換え、中心軸長さｌｎｄｃについて解けばよい。湾曲角度
【００８７】
【数２９】
2019202137.tif 000031
が正のとき、
【００８８】
【数３０】
2019202137.tif 000032
となり、湾曲角度
【００８９】
【数３１】
2019202137.tif 000033
が負のとき、
【００９０】
【数３２】
2019202137.tif 000034
となる。そして、中心軸初期長さｌｎｄからの差分に実施例２と同様の調整ゲインを乗じ、隙間長さ補償量ｌｎｄｄ、
ｌｎｄｄ＝ｒｇｎ（ｌｎｄｃ−ｌｎｄ）      （１５）
とする。これをブロックＫｉｎｅｍａｔｉｃｓから出力されるワイヤ駆動変位、
【００９１】
【数３３】
2019202137.tif 000035
に加算する。これにより、第ｎの相対座標系における近位最小隙間長さがｌｅｎであり、湾曲、旋回角度をθｎ，ζｎとするワイヤ駆動変位が求められる。
【００９２】
つぎに、アクチュエータの制御入力として必要な第ｎ湾曲区間のワイヤ駆動変位ｌｐｎａ，ｌｐｎｂ，ｌｐｎｃを求める。まず、第ｎの相対座標系におけるワイヤ駆動変位の記号を以下のように定める。
【００９３】
【数３４】
2019202137.tif 000036
：第ｎ湾曲区間の遠位端に接続されているａ，ｂ，ｃワイヤの第ｍ湾曲区間のおける駆動変位
これは、
【００９４】
【数３５】
2019202137.tif 000037
となる。これを用いて、ワイヤ駆動変位ｌｐｎａ，ｌｐｎｂ，ｌｐｎｃは、第１〜ｎ区間までの相対座標系におけるワイヤの駆動変位の総和となり、
【００９５】
【数３６】
2019202137.tif 000038
となる。
【００９６】
本実施形態においても、第ｎ番目の湾曲区間の最も近位側のワイヤガイド１６４ｎと、これに隣接する湾曲区間の遠位ワイヤガイド１６０（ｎ＋１）との間の最少隙間長さを一定に保つことができる。この結果、従来技術と比較して連続体ロボットの制御精度を向上させることができる。
【００９７】
（第５の実施形態）
第４の実施形態においては、複数の湾曲区間を有する連続体ロボットに対して湾曲区間毎に相対座標系を導入し、運動学の導出と制御系の設計を行った。しかし、絶対座標系で設定される経路に対して最遠位湾曲区間の角度を追従させる制御では、絶対座標系による制御が簡便となる。そこで、本実施例では、絶対座標系を用いる制御を対象とする。
【００９８】
４．１）モデリング
連続体ロボット１００は、第４の実施形態で説明したものと共通するので、ここでは説明を省略する。
【００９９】
４．２）座標変換
まず、相対角度から絶対座標系における湾曲角度θｎ、旋回角度ζｎを求める手順を示す。第ｎ湾曲区間の相対座標系において各湾曲形状の延長線上に遠位端
【０１００】
【数３７】
2019202137.tif 000039
から長さ１の点
【０１０１】
【数３８】
2019202137.tif 000040
を取ると、その座標は、
【０１０２】
【数３９】
2019202137.tif 000041
となり、絶対座標系における位置（ｘｔｔｎ，ｙｔｔｎ，ｚｔｔｎ）は、回転変換行列を用いて、
【０１０３】
【数４０】
2019202137.tif 000042
となる。ここで、Ｒｚ（θ），Ｒｙ（θ）はそれぞれｚ軸、ｙ軸まわりの回転行列であり、
【０１０４】
【数４１】
2019202137.tif 000043
である。これを用いて、以下の単位ベクトルｅｚｎを定義する。
ｅｚｎ＝（ｘｔｔｎ−ｘｔｎ，ｙｔｔｎ−ｙｔｎ，ｚｔｔｎ−ｚｔｎ）      （２０）
【０１０５】
単位ベクトルｅｚｎは第ｎ湾曲区間遠位端の絶対座標系における方向を表し、ｚ軸からの角度が湾曲角度θｎとなり、単位ベクトルｅｚｎのｘｙ平面への射影のｘ軸からの角度が旋回角度ζｎとなる。絶対角度から相対角度を求めるには、単位ベクトルｅｚｎの方向に第ｎ＋１湾曲区間に対する相対座標系の座標軸ｚｎ＋１をとり、第ｎ湾曲区間の相対座標系のｗｎｚｎ平面上においてｚｎ＋１軸と直行する方向にｘｎ＋１軸をとる。右手系より第ｎ＋１湾曲区間の相対座標系が定義されるので、座標軸と単位ベクトルｅｚｎ＋１の関係から相対角度が求められる。第１湾曲区間は相対角度と絶対角度が等しいので、これを近位端から繰り返すことで、全ての湾曲区間の相対角度を求めることができる。
【０１０６】
４．３）制御系設計
絶対座標系を用いると、中心軸一定制御においては、アクチュエータの制御入力となる第ｎ湾曲区間のワイヤの駆動変位ｌｐｎａ０，ｌｐｎｂ０，ｌｐｎｃ０を以下のように直接的に求めることができる。
【０１０７】
【数４２】
2019202137.tif 000044
【０１０８】
しかし、近位最小隙間長さを旋回角度に依存せず一定とするには、前節の手順により絶対座標系の湾曲、旋回角度θｎ，ζｎから相対座標における湾曲、旋回角度
【０１０９】
【数４３】
2019202137.tif 000045
を求め、それを用いて相対座標系におけるａワイヤ駆動量
【０１１０】
【数４４】
2019202137.tif 000046
を求めたのち、実施例３において式（１３）〜（１７）で示した演算を行う必要があり、演算量が増えてしまう。そこで、本実施例では絶対空間において近似値による簡便な手法を示す。
【０１１１】
まず、第ｎ湾曲区間の湾曲角度θｎをその相対座標における近似的な湾曲角度
【０１１２】
【数４５】
2019202137.tif 000047
に変換することを考える。隣接する２つの湾曲区間である第ｎ−１湾曲区間と第ｎ湾曲区間において、例えば旋回角度ζｎ−１とζｎが等しい場合、相対座標における近似的な湾曲角度は、
【０１１３】
【数４６】
2019202137.tif 000048
とすればよい。一方、例えば旋回角度ζｎ−１とζｎが９０度の差をもつ場合は、
【０１１４】
【数４７】
2019202137.tif 000049
とすればよい。そこで、旋回角度の差分を引数とする関数αを導入し、相対座標における近似的な湾曲角度を以下のように求めればよい。
α＝ｃｏｓ（（ζｎ−ζｎ−１）ｍｏｄ２π））      （２４）
【０１１５】
【数４８】
2019202137.tif 000050
【０１１６】
つぎに、相対座標系におけるａワイヤ駆動量
【０１１７】
【数４９】
2019202137.tif 000051
を座標変換を用いることなく求める。まず、本実施例ではワイヤ駆動変位の記号を以下のように定める。
【０１１８】
ｌｐｎａｍ，ｌｐｎｂｍ，ｌｐｎｃｍ：第ｎ湾曲区間の遠位端に接続されているａ，ｂ，ｃワイヤが仮想的に第ｍ湾曲区間を駆動しているとする駆動変位
これは、
【０１１９】
【数５０】
2019202137.tif 000052
となり、これを用いて第ｎ湾曲区間の相対座標系におけるａワイヤ駆動量
【０１２０】
【数５１】
2019202137.tif 000053
は、
【０１２１】
【数５２】
2019202137.tif 000054
となる。これを、実施例３における式（１３）〜（１５）に代入すると、隙間長さ補償量ｌｎｄｄが求められる。これを用いて、ワイヤ駆動変位ｌｐｎａ，ｌｐｎｂ，ｌｐｎｃは、中心軸一定制御に用いるワイヤの駆動変位ｌｐｎａ０，ｌｐｎｂ０，ｌｐｎｃ０に、第１〜ｎ区間の隙間長さ補償量の総和を加え、
【０１２２】
【数５３】
2019202137.tif 000055
となる。
【０１２３】
上記のとおり、絶対座標系を用いても実施形態３と同様の中心軸一定制御を実現できる。本実施形態においても、第ｎ番目の湾曲区間の最も近位側のワイヤガイド１６４ｎと、これに隣接する湾曲区間の遠位ワイヤガイド１６０（ｎ＋１）との間の最少隙間長さを一定に保つことができる。この結果、従来技術と比較して連続体ロボットの制御精度を向上させることができる。
【０１２４】
（その他）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１２５】
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。
【０１２６】
上記で説明した各実施形態は例示的なものにすぎず、本発明の技術的思想から逸脱しない限りにおいて様々に変更あるいは組み合わせて実施することができる。
【符号の説明】
【０１２７】
１００  連続体ロボット
１１１−１１３  ワイヤ
１６０−１６４  ワイヤガイド
１３０−１３２  アクチュエータ
３００−３０２  制御装置
【図１】
2019202137.tif 000056
【図２】
2019202137.tif 000057
【図３】
2019202137.tif 000058
【図４】
2019202137.tif 000059
【図５】
2019202137.tif 000060
【図６】
2019202137.tif 000061
【図７】
2019202137.tif 000062
【図８】
2019202137.tif 000063
【図９】
2019202137.tif 000064
【図１０】
2019202137.tif 000065