(57)【要約】【課題】分岐を有する経路や空間を含む経路に略沿って進ませるべき連続体ロボットの湾曲可能部の目標姿勢に対する誤差を低減する技術を提供することである。【解決手段】連続体ロボットは、それぞれがワイヤで駆動される複数の湾曲可能部と、運動学モデルに基づいて複数の湾曲可能部の姿勢を制御する制御部を有する。複数の湾曲可能部が、互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて前記運動学モデルを補正する補正値を算出する。補正は、複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用するとともに、複数の補正値を合成する。【選択図】図２
2019532824.tif 000002
【特許請求の範囲】
【請求項１】
それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部と、
前記湾曲可能部の姿勢と前記ワイヤの駆動量とを対応づける運動学モデルに基づいて前記複数の湾曲可能部の姿勢を制御する制御部と、
を有する連続体ロボットであって、
前記連続体ロボットは、前記複数の湾曲可能部が、互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて前記運動学モデルを補正する補正値を算出する運動学モデル補正部をさらに有し、
前記運動学モデル補正部は、前記複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用するとともに、複数の前記補正値を合成することを特徴とする連続体ロボット。
【請求項２】
前記運動学モデル補正部は、前記互いに少なくとも１点の交点を持つ複数の姿勢である分岐を含む経路から複数の目標姿勢を取り出し、前記複数の目標姿勢のうちの目標姿勢に対応する湾曲角度目標値を生成し、前記湾曲角度目標値から前記湾曲可能部の変位を演算し、測定された変位と演算された変位と前記初期値から、前記補正値である補正行列を演算することを特徴とする請求項１に記載の連続体ロボット。
【請求項３】
前記運動学モデル補正部は、空間を含む経路における前記互いに少なくとも１点の交点を持つ複数の姿勢から複数の目標姿勢を取り出し、前記複数の目標姿勢のうちの目標姿勢に対応する湾曲角度目標値を生成し、前記湾曲角度目標値から前記湾曲可能部の変位を演算し、測定された変位と演算された変位と前記初期値から、前記補正値である補正行列を演算することを特徴とする請求項１に記載の連続体ロボット。
【請求項４】
前記運動学モデル補正部は、前記測定された変位と前記演算された変位との誤差を減少すべく最適化手法を用いるアルゴリズムを用いて前記補正値を求めることを特徴とする請求項２または３に記載の連続体ロボット。
【請求項５】
前記運動学モデル補正部は、前記連続体ロボットの前記複数の姿勢のうちの第１の目標姿勢のデータに基づく前記ワイヤの駆動変位により制御された前記湾曲可能部の測定変位を取得し、前記測定変位と前記第１の目標姿勢のデータと前記初期値とを用いて、最急降下法により前記第１の目標姿勢に対する補正値を求め、この手法を、第２の目標姿勢に対しても同様に用いて前記第２の目標姿勢に対する補正値を求めることで、前記複数の姿勢に対してそれぞれ補正値を取得して、複数の前記補正値を合成することを特徴とする請求項４に記載の連続体ロボット。
【請求項６】
前記運動学モデル補正部は、前記第２の目標姿勢に対する最急降下法の補正値の初期値に、前記第１の目標姿勢に対して得られた補正値を用いることを特徴とする請求項５に記載の連続体ロボット。
【請求項７】
前記運動学モデル補正部は、前記最適化手法を反復的に用い、各回の補正値の初期値に、一つ前の回で得られる補正値を用いることを特徴とする請求項５または６に記載の連続体ロボット。
【請求項８】
前記運動学モデル補正部は、全ての前記補正値の平均値を求め、これを前記補正値とすることを特徴とする請求項４から７の何れか１項に記載の連続体ロボット。
【請求項９】
前記制御部は、前記補正値の逆値を前記湾曲可能部の湾曲角度目標値に乗じて補正された湾曲角度を求め、前記補正された湾曲角度に基づいて演算される前記ワイヤの駆動変位により前記湾曲可能部を制御することを特徴とする請求項２から８の何れか１項に記載の連続体ロボット。
【請求項１０】
前記制御部は、前記逆値として、前記補正値である補正行列の逆行列を用いることを特徴とする請求項９に記載の連続体ロボット。
【請求項１１】
前記制御部は、湾曲形状の追加目標値を前記湾曲角度目標値に加算することにより、予め設定された湾曲形状のみではなく、実時間の操作による湾曲形状の制御を行うことを特徴とする請求項９または１０に記載の連続体ロボット。
【請求項１２】
前記制御部は、先頭追従制御を行うことを特徴とする請求項９から１１の何れか１項に記載の連続体ロボット。
【請求項１３】
前記運動学モデル補正部は、前記分岐を含む経路における前記先頭追従制御による湾曲可能部の変位を測定して得られた変位と前記演算された変位との誤差が最大値となる変位における湾曲可能部の姿勢を目標姿勢に加え、前記加えた姿勢を含む目標姿勢について、前記補正値である補正行列を演算することを特徴とする請求項１２に記載の連続体ロボット。
【請求項１４】
前記運動学モデル補正部は、前記誤差が所定の条件を満たすまで、前記湾曲可能部の姿勢を目標姿勢に加えて前記補正行列を演算することを繰り返すことを特徴とする請求項１３に記載の連続体ロボット。
【請求項１５】
それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部と、前記湾曲可能部の姿勢と前記ワイヤの駆動量とを対応づける運動学モデルに基づいて前記複数の湾曲可能部の姿勢を制御する制御部と、を有する連続体ロボットにおける前記運動学モデルの補正方法であって、
前記複数の湾曲可能部が、互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて前記運動学モデルを補正する補正値を算出する算出ステップと、
複数の前記補正値を合成する合成ステップと、を有し、
前記算出ステップにおいて、前記複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用することを特徴とする補正方法。
【請求項１６】
前記算出ステップにおいて、前記互いに少なくとも１点の交点を持つ複数の姿勢である分岐を含む経路から複数の目標姿勢を取り出し、前記複数の目標姿勢のうちの目標姿勢に対応する湾曲角度目標値を生成し、前記湾曲角度目標値から前記湾曲可能部の変位を演算し、測定された変位と演算された変位と前記初期値から、前記補正値である補正行列を演算することを特徴とする請求項１５に記載の補正方法。
【請求項１７】
前記算出ステップにおいて、空間を含む経路における前記互いに少なくとも１点の交点を持つ複数の姿勢から複数の目標姿勢を取り出し、前記複数の目標姿勢のうちの目標姿勢に対応する湾曲角度目標値を生成し、前記湾曲角度目標値から前記湾曲可能部の変位を演算し、測定された変位と演算された変位と前記初期値から、前記補正値である補正行列を演算することを特徴とする請求項１５に記載の補正方法。
【請求項１８】
前記算出ステップにおいて、前記測定された変位と前記演算された変位との誤差を減少すべく最適化手法を用いるアルゴリズムを用いて前記補正値を求めることを特徴とする請求項１６または１７に記載の補正方法。
【請求項１９】
前記分岐を含む経路における先頭追従制御による湾曲可能部の変位を測定する測定ステップと、
前記測定された変位と前記演算された変位との誤差が最大値となる変位における湾曲可能部の姿勢を目標姿勢に加える追加ステップと、
前記加えた姿勢を含む目標姿勢について、前記補正値である補正行列を演算する演算ステップと、
をさらに含むことを特徴とする請求項１６から１８の何れか１項に記載の補正方法。
【請求項２０】
前記誤差が所定の条件を満たすまで、前記測定ステップから前記演算ステップを繰り返すことを特徴とする請求項１９に記載の補正方法。
【請求項２１】
それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部を有する連続体ロボットの制御方法であって、
請求項１６から２０の何れか１項に記載の補正方法を利用し、前記補正値の逆行列を前記湾曲可能部の湾曲角度目標値に乗じて補正された湾曲角度を求め、
前記補正された湾曲角度に基づいて演算される前記ワイヤの駆動変位により前記湾曲可能部を制御することを特徴とする制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、連続体ロボット、連続体ロボットの運動学モデルの補正方法、および連続体ロボットの制御方法に関する。
【背景技術】
【０００２】
連続体ロボットは、柔軟性のある構造を持つ複数の湾曲区間（湾曲可能部）からなり、その湾曲区間を変形ないし移動させることにより全体の形状が制御される。このロボットは、剛体リンクにより構成されるロボットに対して、主に二つの優位性を持つ。一つ目は、連続体ロボットは、剛体リンクロボットが嵌まり込んでしまうような、狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。二つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、内視鏡のシースやカテーテルなど医療の分野のロボットや、レスキューロボットなど極限作業ロボットへの応用が期待されている。
【０００３】
こうした技術について、特許文献１は、先頭追従制御に関して、最遠位端の湾曲角度を後続の湾曲区間に時間差をもって連続的に伝播させる方法を開示している。特許文献１は、また、ロボットの湾曲形状と進むべき経路との面積を評価し湾曲形状を制御することで経路に精度良く追従する方法を示している。特許文献２は、分岐部を持つ血管ツリー情報から内視鏡の経路を生成し、その経路に基づいて内視鏡をガイドするようにロボットを制御する技術を開示している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許第８，２１９，２４６号明細書
【特許文献２】特表２０１３−５４１３６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、経路が分岐部を持つと、連続体ロボットが進みうる経路の数は分岐部数の階乗で増加する。特許文献１に示されるような、経路に対して面積を評価することで経路追従の精度を向上させる手法を、分岐部を有する経路に適用すると、その処理に必要とされる時間や記憶容量が膨大になってしまう。また、特許文献２は、経路追従に対するロボットの制御精度の性能を向上させる方法を開示していない。
【課題を解決するための手段】
【０００６】
本発明の一側面の連続体ロボットは、それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部と、前記湾曲可能部の姿勢と前記ワイヤの駆動量とを対応づける運動学モデルに基づいて前記複数の湾曲可能部の姿勢を制御する制御部と、を有する連続体ロボットであって、前記複数の湾曲可能部が、互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて前記運動学モデルを補正する補正値を算出する運動学モデル補正部をさらに有し、前記運動学モデル補正部は、前記複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用するとともに、複数の前記補正値を合成する。
【０００７】
本発明の他の側面の補正方法は、それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部と、前記湾曲可能部の姿勢と前記ワイヤの駆動量とを対応づける運動学モデルに基づいて前記複数の湾曲可能部の姿勢を制御する制御部と、を有する連続体ロボットにおける前記運動学モデルの補正方法であって、前記複数の湾曲可能部が、互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて前記運動学モデルを補正する補正値を算出し、前記複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用するとともに、複数の前記補正値を合成する。
【０００８】
本発明の他の側面の制御方法は、それぞれがワイヤで駆動され、湾曲可能に構成された複数の湾曲可能部を有する連続体ロボットの制御方法であって、前記補正方法を利用し、前記補正値の逆行列を前記湾曲可能部の湾曲角度目標値に乗じて補正された湾曲角度を求め、前記補正された湾曲角度に基づいて演算される前記ワイヤの駆動変位により前記湾曲可能部を制御する。
【発明の効果】
【０００９】
本発明の一側面によれば、処理時間や記憶容量の増大を抑制しつつ連続体ロボットの湾曲可能部の目標姿勢に対する誤差を低減することができる。
【００１０】
本発明のさらなる特徴は、本発明の例示的な実施形態を示す添付の図を併用して、以下の詳細な説明から明らかになるであろう。
【図面の簡単な説明】
【００１１】
【図１】図１は第１の実施形態における先頭追従制御を示す図である。
【図２】図２は第１の実施形態における運動学モデルを示す図である。
【図３】図３は第１の実施形態における運動学モデルを示す図である。
【図４】図４は第１の実施形態における運動学モデルを示す図である。
【図５】図５は第１の実施形態における運動学モデルを示す図である。
【図６】図６は第１の実施形態における運動学モデルを示す図である。
【図７Ａ】図７Ａは第１の実施形態における経路抽出を示す図である。
【図７Ｂ】図７Ｂは第１の実施形態における経路抽出を示す図である。
【図７Ｃ】図７Ｃは第１の実施形態における経路抽出を示す図である。
【図８】図８は第１の実施形態における最適化アルゴリズムを示す図である。
【図９】図９は第１の実施形態における制御フロー図である。
【図１０】図１０は第１の実施形態における制御系を示すブロック図である。
【図１１】図１１は第１の実施形態における変位測定装置を示す図である。
【図１２】図１２は第１の実施形態におけるフロベニウスノルムを示す図である。
【図１３Ａ】図１３Ａは第１の実施形態における実験結果を示す図である。
【図１３Ｂ】図１３Ｂは第１の実施形態における実験結果を示す図である。
【図１４Ａ】図１４Ａは第１の実施形態における実験結果を示す図である。
【図１４Ｂ】図１４Ｂは第１の実施形態における実験結果を示す図である。
【図１５Ａ】図１５Ａは第２の実施形態における経路抽出を示す図である。
【図１５Ｂ】図１５Ｂは第２の実施形態における経路抽出を示す図である。
【図１５Ｃ】図１５Ｃは第２の実施形態における経路抽出を示す図である。
【図１５Ｄ】図１５Ｄは第２の実施形態における経路抽出を示す図である。
【図１６】図１６は第２の実施形態における代表形状決定のためのフロー図である。
【図１７】図１７は第３の実施形態におけるフロー図である。
【図１８Ａ】図１８Ａは第３の実施形態における代表形状追加の場合の誤差を示す図である。
【図１８Ｂ】図１８Ｂは第３の実施形態における代表形状追加の場合の誤差を示す図である。
【図１９Ａ】図１９Ａは第３の実施形態における更なる代表形状追加の場合の誤差を示す図である。
【図１９Ｂ】図１９Ｂは第３の実施形態における更なる代表形状追加の場合の誤差を示す図である。
【図２０】図２０は運動学的誤差のノルムを示す図である。
【発明を実施するための形態】
【００１２】
本発明では、例えば湾曲可能部間の連成を考慮したモデルを用いて運動学モデルを補正し、その補正された運動学モデルに基づいて、連続体ロボットを構成したり、連続体ロボットの制御方法を構築したりすることができる。ここでの運動学モデルは、湾曲可能部の姿勢と、可撓性を有する線状体であるワイヤの駆動量とを対応づける運動学モデルである。運動学モデル補正部が、この運動学モデルを、前記複数の湾曲可能部が互いに少なくとも１点の交点を持つ複数の姿勢を取る場合のそれぞれについて、目標姿勢と測定された姿勢とに基づいて補正値を算出することで補正する。「互いに少なくとも1点の交点を持つ複数の姿勢」とは、以下で説明する連続体ロボットの基端を固定した状態で基端以外の位置で、連続体ロボットが取る姿勢が交差することを意味する。基端から、基端と連続体ロボットの最遠位端との間のある点までが共通の姿勢であって、当該点から最遠位端までの湾曲区間の姿勢が異なるような場合も、互いに少なくとも１点の交点を持つ複数の姿勢であると言える。この状態を以下では特に、「分岐」と称することもある。なお、以下では、姿勢のことを「形状」とも表記することがある。補正では、複数の姿勢のうちの少なくとも１つにおける補正結果を、別の姿勢における前記運動学モデルを補正する場合の初期値として利用するとともに、複数の補正値を合成する（例えば平均値を求める）。
【００１３】
制御方法では、湾曲可能部の目標姿勢のデータ（湾曲目標値）と前記補正された運動学モデルにおける補正値の逆値（補正値との積が１となる値）とを用いて、前記目標姿勢を達成するワイヤの駆動量を取得して、湾曲可能部を制御する。湾曲形状の追加目標値を、逐次、前記湾曲目標値に加算することにより、予め設定された湾曲量のみではなく、実時間の操作による湾曲形状の実時間制御を行うこともできる。これを利用して先頭追従制御を行うことができる。
【００１４】
（第１の実施形態）
第１の実施形態では、分岐を含む経路に対する先頭追従制御の適用を行う。図１に示すように、基端部がロボット基台部１４０に接続された複数の湾曲区間（湾曲可能部）を含む連続体ロボット１００があるとする。基台部１４０を、時間ｔの経過に伴って図中ｚ軸方向に移動させたときに、連続体ロボット１００に経路Ｐに沿った姿勢を取らせる制御を行う。この際に、連続体ロボット１００の最遠位端Ｅの後続の湾曲区間が、最遠位端Ｅが通過した位置を通過するように連続体ロボット１００の姿勢を制御することを、「先頭追従制御」と呼ぶ。これにより、連続体ロボットは体腔のような狭小な空間をすり抜けるように進むことができる。
【００１５】
ロボットの姿勢誤差を補正するためには、ロボットの目標角度（目標姿勢）を補正するためのテーブルを予備実験により取得する方法（以下、「テーブル補正」と略す）が知られている。これを先頭追従制御に適用するためには、経路を進むべきロボットの湾曲区間の形状のすべてに対して、それを補正するためのテーブルを取得する必要がある。テーブル補正を、分岐を有する経路に対して適用するには、テーブルを作成するための膨大な時間とメモリを要するという問題がある。
【００１６】
本発明では、分岐を有する経路から複数の分岐なしの経路を抽出する。そして、それに沿う湾曲形状の中から幾つかの形状を選び、連続体ロボットの運動学モデルを補正する係数を算出する。この補正係数は、経路における連続体ロボットの湾曲形状を測定し、最適化アルゴリズムにより求めることができる。この測定に用いる湾曲形状を「代表形状」と呼ぶ。経路の一部を用いて前記最適化を行うことで運動学モデルを補正し、ロボットの姿勢誤差を低減することができることから、膨大なテーブルを必要としない。
【００１７】
本実施形態のアルゴリズムを図８にブロック図で示す。ブロック図中の記号の定義を次に示す。分岐を有する経路をTraで表し、抽出器は、分岐を有する経路から分岐なしの経路を抽出するブロックである。抽出されたb本の分岐なしの経路をTra1,...,Trabで示す。Fitブロックは、それぞれの分岐なしの経路に対して、先頭追従制御を行うための目標角度を最適化する。さらにFitブロックは、湾曲目標角度行列Θrefから、補正行列Γの最適化のための湾曲角度目標値ベクトルθrefζを代表形状θrepとして選択する。Pは連続体ロボットを表し、Kは各湾曲区間の遠位端の角度を目標値としワイヤ駆動変位を求めるブロックである。
【００１８】
まず、第１の代表形状ベクトルθrep[1]を設定し、ワイヤ変位演算ブロックKにより、ワイヤ駆動変位lp[1]を求める。次に、ロボットPに対して、ワイヤ駆動変位を与えて制御し、図１１に示す湾曲区間遠位端の変位測定系により、ロボットの湾曲形状の測定変位xex[1],zex[1]を取得する。この測定変位と代表形状θrep[1]と補正行列の初期行列Γ0とを用いて、最急降下法演算ブロックSDMにより第１の代表形状に対する補正行列Γ[1]1を求める。次に、第２の代表形状に対して、同様に補正行列Γ[2]1を求めるが、本実施形態では、第２の代表形状に対する最急降下法の初期値に第１の代表形状に対して得られた補正行列Γ[1]1を用いる。これにより、補正行列が局所的最小値となることを回避することができる。そして、a通りの代表形状に対してそれぞれ補正係数行列Γ[a]1を求める。ここまでが第１回目の反復試行となる。
【００１９】
次に第２回目の反復試行を行うが、その補正行列の初期値は第１回目の反復で得られる補正行列Γ[a]1とする。そして、第１回目の試行と同様に第２回目の試行が行われる。この試行をh回反復し、補正係数が十分に収束したかをブロックCJBによって判断する。全ての補正係数の平均値を平均値演算ブロック１／(ah)により求め、これを補正行列Γとする。平均値は、全ての補正係数を合成することで得られる他のものに置き置えてもよい。例えば、中央値、代表形状の態様に応じて決められた重みを考慮した重み付きの平均値、などでもよい。結果的に充分な補正効果が得られるものであれば、どの様な合成値でもよい。前記補正係数Γ[a]1などは、複数の代表形状（姿勢）のうちの少なくとも１つにおける補正結果を用いて別の代表形状（姿勢）における前記運動学モデルを補正する場合の初期値として利用することができる。
【００２０】
以下に、連続体ロボットの運動学モデルの導出と、分岐を有する経路に対する補正行列最適化アルゴリズム、さらには制御系について記述し、実験によって得られた応答を示す。第１の実施形態では、湾曲可能部である湾曲区間について区分的に曲率を一定と仮定する運動学モデルに加えて、複数の機構的要因により生ずる隣接湾曲区間の間の連成のモデルを導入して前記運動学モデルを補正する方法を示す。そして、それによる連続体ロボットの駆動・制御を向上させる手法を示す。連続体ロボットの運動学モデルは、湾曲区間の目標姿勢のデータを与えたときに、連続体ロボットの機構に基づいて該データから導かれるアクチュエータ（制御部）の駆動量により如何なる湾曲区間の姿勢が実現されるかを呈示する。
【００２１】
（１．連続体ロボットのモデリング）
（１.１  運動学モデルの導出）
図２は、本実施形態で用いる６つの湾曲区間１０１〜１０６を含む連続体ロボット１００の概略図を示す。連続体ロボット１００は、たとえば円筒形状をしており、その空洞部にカメラや処置具などを挿通させることができる構成となっている。連続体ロボットの各湾曲区間の遠位端１２１〜１２６に駆動用ワイヤ１１１〜１１６がそれぞれ接続され、それらのワイヤを、ロボット基台部１４０に設置するアクチュエータ１３１〜１３６でそれぞれ押し引きすることにより湾曲区間１０１〜１０６の姿勢が制御される。図２に示す例では、駆動用ワイヤ１１１〜１１６が湾曲区間１０１〜１０６の遠位端１２１〜１２６に接続される位置が、中心軸を挟んで交互に反対側になっているが、ワイヤの湾曲区間への接続位置はこの形態に限らない。ワイヤの押し引きにより湾曲区間の姿勢が制御できれば接続箇所の位置は問わない。基台部１４０はz軸方向に自由度を持ち、その移動変位が検出可能である。図３に第１湾曲区間１０１を取り上げ、湾曲区間の筐体とワイヤ１１１からなる構造の模式図を示す。図１〜図３において、筐体の中心軸は破線ないし一点鎖線で示す。
【００２２】
以下の説明における記号の定義を次に示す。ln：第n湾曲区間のアーム筐体の長さ、rn：第n湾曲区間のアーム筐体の中心軸からワイヤまでの変位、e：ロボットの湾曲区間数、θn：第n湾曲区間の遠位端の角度、ρn：第n湾曲区間の曲率半径、θrefn：第n湾曲区間の遠位端の目標角度、lpn：第n湾曲区間のワイヤの駆動変位、xtn,ztn：第n湾曲区間の遠位端の座標、c：ロボットの評価点数、xi,zi：ロボットを長手方向にc分割するときの第i番目の座標、zb：基台部の変位。
【００２３】
次のような仮定をおき、図４に示す湾曲区間数をnとする連続体ロボットの運動学モデルを導出する。
１．筐体およびワイヤは紙面内のみに変形する。
２．各湾曲区間において、筐体およびワイヤは曲率一定に変形する。
３．筐体およびワイヤのねじり変形は考慮しない。
４．筐体およびワイヤは長手方向に変形しない。
５．筐体にワイヤガイドが設けられ、筐体中心軸の円弧とワイヤによって作られる円弧との中心が常に等しい。
６．筐体とワイヤ間の摩擦は考慮しない。
【００２４】
まず、ワイヤ駆動変位と湾曲区間の遠位端の角度との関係を導出する。第１湾曲区間のみを考えると、ワイヤの駆動量lp1と第１湾曲区間の遠位端の角度θ1の関係は、下記の（１）式である。ここで、下記の（２）式が成り立つから、式（１）、式（２）より、下記の式（３）が得られる。次に、第n湾曲区間のワイヤ駆動変位lpnと遠位端の角度θnの関係を導出する。ただし、nは２以上とする。第n湾曲区間の湾曲相対角度である上ティルダ付きθnを下記の式（４）のように定義する。そして、図４（図５も参照）に示すように原点を（xtn-1,ztn-1）とし、θn-1方向およびその直交方向からなる相対座標系xn-znをとる。すると、相対座標系xn-znにおけるワイヤの駆動変位である上ティルダ付きlpnと第ｎ湾曲区間の遠位端の相対角度である上ティルダ付きθnの関係は、下記の式（５）である。第n湾曲区間のワイヤ駆動変位lpnは、第１〜第n-１区間までの相対座標系における第n湾曲区間を駆動するためのワイヤの変位の総和となり、下記の式（６）となる。
【数１】
2019532824.tif 000003
【００２５】
これより、第n湾曲区間の遠位端の角度θnは、ワイヤ駆動変位lpnによってのみ決まり、第１〜第n-1湾曲区間の角度には依存しないことが分かる。
【００２６】
次に、第n湾曲区間の遠位端角度と遠位端座標の関係を導出する。まず、第１湾曲区間を考える。ρを曲率半径とすると、第１湾曲区間の遠位端の座標（xt1,zt1）は、下記の式（７）および式（８）となる。ここで、上記の式（２）を式（７）および式（８）に代入すると、下記の式（９）および式（１０）が得られる。ここで、第n湾曲区間の遠位端角度と遠位端座標の関係を導出する。ただし、nは２以上とする。相対座標系xn-znにおける湾曲区間の遠位端の座標である上ティルダ付きxtn、および上ティルダ付ztnは下記の式（１１）および式（１２）となる。
【００２７】
これより、絶対座標系での遠位端の座標（xtn,ztn）は回転変換行列を用いて、下記の式（１３）となる。また次節において、ロボット全体をα倍に細分割する座標を、最適化アルゴリズムの評価点として用いる。このとき、評価点の総数はc=αeであり、第iの評価点の座標（xi,zi）は、下記の式（１４）と求まる。ただし、QはQ=[i／α]から求まる商であり、RはR=i mod αから求まる剰余である。
【数２】
2019532824.tif 000004
【数３】
2019532824.tif 000005
【数４】
2019532824.tif 000006
【００２８】
（１.２  隣接区間連成モデルによる運動学モデル補正）
前節では、仮定１〜６をおき運動学モデルの導出を行ったが、ロボットの曲げ方向には柔軟で長手方向には高剛性となる筐体およびワイヤは構造設計が難しく、仮定３、４は厳密には成立しにくい。そのため、ワイヤ駆動変位に対する湾曲角度の上記関係式（６）が成立せず、制御後の実際の湾曲角度は目標角度から誤差が生じてしまう。さらに、この影響は、ワイヤを駆動する湾曲区間のみにとどまらず、他の区間の湾曲角度に誤差を生じさせ、連成問題の原因となる。そこで、本実施形態では、湾曲区間の連成に対する仮定を加え、運動学モデル誤差を補正する。
【００２９】
本実施形態では、湾曲区間の連成に対する次の仮定を加え、運動学モデル誤差を補正する。
７．１つの湾曲区間を駆動するとき、全ての湾曲区間は連成する。
これを行列表記する下記の式（１５）のようになり、本実施形態ではこの行列を補正行列Γと表す。これより、補正される各湾曲区間遠位端の座標（xgtn,zgtn）は、下記の式（１６）となる。
【数５】
2019532824.tif 000007
【数６】
2019532824.tif 000008
【００３０】
より具体的な例を挙げると次のようになる。第n湾曲区間を、湾曲角度である上ティルダ付きθnとなるように駆動するとき、次の様な連成が生じる。すなわち、第n-１湾曲区間および第n+１湾曲区間の湾曲角度はそれぞれγpn＊（上ティルダ付きθn）、γdn＊（上ティルダ付きθn）増加し、第n湾曲区間の湾曲角度は（γpn＋γdn）＊（上ティルダ付きθn）減少する。ここで、γpnおよびγdnは正数の補正係数である。つまり、第iの湾曲可能部の湾曲相対角度に係数γp[i]を乗じた角度は、隣接する第i-１の湾曲可能部の湾曲相対角度に加算される。また、前記第iの湾曲可能部の湾曲相対角度に別の係数γd[i]を乗じた角度は、隣接する第i+１の湾曲可能部の湾曲相対角度に加算され、前記第iの湾曲可能部の湾曲相対角度からは、これに（γp[i]+γd[i]）を乗じた角度が減算される。こうしたモデルを用いて補正値を求める（ただし、iは２以上で、前記湾曲可能部の数より１少ない数以下）。これを行列表記すると、この補正行列Γでは、i行i列が１-γd[i]-γp[i]であり、i行i-１列がγd[i-1]であり、i行i+１列がγp[i+1]である。
【００３１】
ロボット全体をα倍に細分割するときの、第iの評価点の座標（xgi,zgi）は、前記の式（１４）においてθ、上ティルダ付きθをθg、上ティルダ付きθgと置き換えればよく、省略する。
【００３２】
（１.３  補正係数最適化）
前節で示した運動学モデル補正行列を解析的に求めることは難しい。それは、湾曲角度の連成は、長手方向の圧縮引張力により筐体およびワイヤが変形することにより生ずるが、その力は湾曲区間によってワイヤの通る本数が異なったり、筐体の剛性が湾曲角度により非線形に変動したりするためである。さらには、仮定１、２、５、６についても、実際は完全には成り立たなく、湾曲角度の誤差の原因となる。そこで、本実施形態では実験的に湾曲形状を測定し、測定値と運動学モデルとの誤差から最適化の手法である最急降下法を用いて補正行列Γを求める。このとき、補正行列Γの最適化に好適な湾曲形状を一つ選ぶのは不可能であるため、本実施形態では、分岐を有する経路からa通りの代表形状を選び、h回の反復による最適化を行う。これを拡張最急降下法とよぶ。
【００３３】
拡張最急降下法のアルゴリズムを、前述した図８にブロック図で示す。ブロック図中の記号の定義を以下に示す。分岐を有する経路をTraで表し、抽出器ブロックは分岐を有する経路から分岐なしの経路を抽出する。抽出されたb本の分岐なしの経路をTra1,...,Trabで示す。Fitブロックは、それぞれの分岐なしの経路に対して、先頭追従制御を行うための目標角度を最適化する。さらにFitブロックは、湾曲目標角度行列Θrefから、補正行列Γの最適化のための湾曲目標角度θrefζを代表形状として選択するブロックである。抽出器ブロックおよびFitブロックの詳細な手順は次章で示す。ここで、代表形状として経路終点を選べば、終点付近での湾曲形状を精度よく補正できる。さらに、経路中間付近での湾曲形状を改善するときは、経路中間付近での湾曲目標角度を代表形状として追加すればよい。
【００３４】
記号[k]は第k(≦a)の代表形状を、記号jは第j(≦h)の反復的試行を表す。第kの代表形状ベクトルθrep[k]を下記の式(１７)と、第kの代表形状に対するワイヤ駆動変位lp[k]を下記の式(１８)と、ロボットの測定変位xex[k],zex[k]を下記の式(１９)と表記する。また、SDMは最急降下法アルゴリズムを表し、下記の式(２１)の評価関数を最小化する補正係数ベクトルγ[k]jを最適化により求めるブロックである。なお、補正行列Γ[k]jは第kの代表形状に対する反復回数jの補正行列である。
【００３５】
前述したように、これらのブロックにより、拡張最急降下法により補正行列Γを得るには、まず、第１の代表形状ベクトルθref[1]を設定し、式(６)の演算を行うワイヤ変位ブロックKにより、ワイヤ駆動変位lp[1]を求める。次に、ロボットPに対して、ワイヤ駆動変位を与え制御し、ロボットの湾曲形状の測定変位xex[1],zex[1]を取得する。この測定変位と、代表形状θrep[1]と、補正行列の初期行列Γ0を用いて、最急降下法演算ブロックSDMにより第１の代表形状に対する補正行列Γ[1]1が求まる。次に、第２の代表形状に対して、同様に補正行列Γ[2]1を求めるが、本実施形態では、第２の代表形状に対する最急降下法の初期値に第１の代表形状に対して得られた補正行列Γ[1]1を用いる。これより、補正行列が局所的最小値となることを回避することができる。そして、図８に示すように、a通りの代表形状に対して補正行列Γ[k]1を求め、ここまでを第１回目の反復試行とする。次に第２回目の反復試行を行うが、その補正行列の初期値は第１回目の反復で得られる補正行列とする。そして、第１回目の試行と同様に第２回目の試行が行われる。この試行をh回反復し、補正行列が十分に収束したかをブロックCJBによって判断する。もしくは、反復回数は試行錯誤により予め決定されていてもよい。そして、全ての補正行列の平均値を下記の式（２２）のように求め、これを補正行列Γとする。
【数７】
2019532824.tif 000009
【数８】
2019532824.tif 000010
【００３６】
（２．制御系設計）
本章では、まず図８の拡張最適化アルゴリズムに抽出器ブロックで示す分岐を有する経路から、分岐なしの経路を抽出する方法を示し、さらに、Fitブロックで示す分岐なしの経路に対する先頭追従制御の目標角度最適化法を示す。次に、拡張最適化アルゴリズムにより求まる補正行列Γの逆行列Γ-1を用いる湾曲角度制御の補償方法を示す。
【００３７】
（２.１  分岐を含む経路から分岐なしの経路を抽出）
分岐を含む経路に対して先頭追従制御を行うために、本実施形態では、b点の終点を有する分岐を有する経路から、b本の分岐なしの経路を抽出する。また、抽出された分岐なしの経路は、それぞれの分岐なしの経路の共通部分を含み、前節で追加された接線部分を含むものとする。例として、図７Ａに二分岐の経路を示す。この分岐を有する経路から、２本の分岐なしの経路を抽出する。図７Ｂは第１の分岐なしの経路を示し、図７Ｃは第２の分岐なしの経路を示す。
【００３８】
（２.２  先頭追従制御の目標角度最適化）
本節では、分岐なしの経路に対する目標角度最適化の方法を示す。先頭追従制御は、予め経路を定められていることは必須ではなく、最遠位端の湾曲角度を後続の湾曲区間に時間差をもって連続的に伝播させれば良い。しかし、図１に破線で示すように経路の全体が予め設定されているときは、基台部のz方向の変位に応じて湾曲形状を最適化することが可能である。これにより、連続体ロボットでは、湾曲角度を伝播させる先頭追従制御よりも経路誤差を少なくすることができる。以下に手順を示す。
【００３９】
本実施形態では、図６の一点鎖線に示すように、経路がロボット全長と等しい例を考える。まず、図６の破線に示すように、経路の近位端に接線を追加し、その終端に座標系の原点をとる。接線の長さは、ロボット全長と等しい長さとすればよい。以下、本実施形態では、この追加した接線と経路を合わせて全経路と記す。次に、全経路を等速スプライン補間などにより等間隔に２c分割し、分割された節に経路評価点P1(xtr1,ztr1),...,P2c(xtr2c,ztr2c)を設定する。先頭追従制御は、基台部の変位がzb=ztr1=０ mから開始され、zb=ztrc=nl mにおいて終了する。
【００４０】
基台の前進に伴い、全経路に沿うような連続体ロボットの湾曲形状を求めるには、まず、経路評価点Pζ(１<ζ≦c)を開始点として、全経路上からc個の経路評価点を取り出す。この経路評価点に連続体ロボットの湾曲形状が漸近するためには、上記式(１４)に示したロボット上のc個の評価点を用いて、それぞれの距離の総和を評価関数（下記の式（２３））を最小化すればよい。本実施形態では、この座標（xi,zi）を実現する湾曲角度目標値ベクトルθrefを、第n湾曲区間に対する目標角度θrefnを成分として、下記の式（２４）のように定義する。
【００４１】
ここで、基台部が第ζの経路評価点Pζにあるときの、湾曲目標角度をθrefζと書くと、先頭追従制御のためには、e行c列の湾曲目標角度行列Θref（下記の式（２５））を得ればよい。
【００４２】
なお、湾曲区間数が有限であるため、経路によっては式(２３)の評価関数は０にはならない。そこで、最適化により得られた湾曲角度目標値による形状と全経路の差を機構的誤差と定義し、その湾曲角度目標値とワイヤ駆動により制御された湾曲角度の誤差を制御的誤差と定義する。また、この手順で抽出される第p(p≦b)の分岐なしの経路に対し、最適化により求まる湾曲目標角度行列をΘrefpと表記する。また、経路がロボット全長よりも短い場合は、（１）近位端に追加する接線を延長する、もしくは、（２）遠位端に接線を追加する、ことで前述のアルゴリズムを用いて湾曲角度目標値を生成することが可能である。すなわち、（１）では、経路の近位端に追加する接線の長さを、（ロボットの全長+ロボットの全長-経路の長さ）、と設定すればよい。また、（２）では経路の遠位端に接線を追加し、その長さを、（ロボットの全長-経路の長さ）に設定すればよい。ただし、（２）では基台部の変位が経路の長さと等しくなる座標において先頭追従制御を終了する必要がある。
【数９】
2019532824.tif 000011
【００４３】
（２.３  分岐を有する経路に対する先頭追従制御）
２.１節で分岐を有する経路から分岐なしの経路を抽出する手順を示し、２.２節でその分岐なしの経路に対して先頭追従制御を行うための湾曲角度目標値を導出した。次に、その分岐を有する経路に対して、例えば順次終点に到達するように先頭追従制御を行うためのフローを、図９に示す。まず、p=１とし、第pの分岐なしの経路に対する湾曲目標角度行列Θrefpを読み込み、先頭追従制御を行う。そして、最遠位端が第p経路と第p+１経路の分岐点を通過し、かつ、基台変位zbの速度が負であれば、p=p+１とし、次の分岐なしの経路に対する先頭追従制御を開始する。その際、p<bであることが必要である。p=bであれば分岐点のすべてを通過したと判断し、基台が原点に戻り次第、制御を終了する。このフローでは、基台変位を操縦者が任意に操作しても、基台がモータ駆動されており、その動作が予め設定されていてもよい。
【００４４】
（２.４  補正された運動学モデルの制御系への適用）
１.３節に示す拡張最急降下法により求められる運動学モデル補正行列Γは、ロボットの運動学モデルを補正し、その湾曲形状を算出するのみではなく、先頭追従制御に適用可能である。まず、前節で求めた湾曲目標角度行列Θrefpから、第pの分岐なしの経路における基台変位zbに応じて目標湾曲形状ベクトルθrefζを取り出す。そして、第n湾曲区間に対する補正目標角度θmodnからなる補正湾曲形状ベクトルθmodを下記の式（２６）と定義する。すると、ロボットを目標湾曲形状に制御するには、補正行列Γの逆行列を用い、下記の式（２７）のように補正湾曲形状ベクトルを求めればよい。
【００４５】
そして、この補正目標角度を絶対座標系に変換し、上記式(６)の角度θに代入し、ワイヤ駆動変位を求める。さらに、本実施形態の制御系では、湾曲形状の追加目標ベクトルθcomを目標湾曲形状ベクトルに加算することにより、予め設定された湾曲形状のみではなく、実時間の操作による湾曲形状の制御に運動学モデル補正を適用することができる。制御系のブロック図を図１０に示す。
【数１０】
2019532824.tif 000012
【００４６】
（３．実験）
本節では、分岐を有する経路に対する運動学モデル補正アルゴリズムの有効性を示す。実験で用いるロボットのパラメタは、アーム筐体の長さl1〜l6=０.０１０m、ロボットの湾曲区間数e=６である。また、第n湾曲区間のアーム筐体の中心軸からワイヤまでの変位は、
r1=r3=１.３２×１０-3m、
r2=r4=-１.３２×１０-3m、
r5=１.４×１０-3m、
r6=-１.４×１０-3mである。
実験では、図１１に示すように、連続体ロボット１００の湾曲区間の遠位端にマーカ２０１〜２０６を設け、各湾曲区間の遠位端の変位（下記の式（２８））を画像撮影装置２１０によって取得する。拡張最急降下法では、評価点数c=６０とする。そのため、湾曲区間の遠位端以外の評価点は、湾曲区間が一定曲率であると仮定し補間により求める。代表湾曲姿勢として、図７Ｂに示した第１の分岐なしの経路のそれぞれの終点での姿勢と図７Ｃに示した第２の分岐なしの経路の終点での姿勢をそれぞれ選び、下記の式（２９）および（３０）にそれぞれ示す。
【００４７】
また、拡張最急降下法アルゴリズムでは反復回数h=１０とする。図１２は、（１.３）節で示した拡張最急降下法アルゴリズムによる、反復試行回数に対する補正行列Γの最適化によるフロベニウスノルムの応答を示す。振動的であるが、収束しており、反復により補正行列が最適化されていることが分かる。
【００４８】
まず、代表形状として選んだ第１の分岐なしの経路の終点における姿勢の運動学モデル補正の応答を図１３Ａに示し、代表形状として選んだ第２の分岐なしの経路のそれぞれの終点における姿勢の運動学モデル補正の応答を図１３Ｂに示す。補正された運動学モデルによる応答を実線で、実験により測定される湾曲区間の遠位端の変位をアスタリスクで、比較として補正前の運動学モデルの応答を破線で示す。図１３Ａおよび図１３Ｂより、（１.１）節の仮定１〜６をおいて導出した運動学モデルでは、実験による応答との誤差が生じてしまうが、その実験応答を用いて補正行列Γを最適化する運動学モデル補正により、実験応答に対する誤差を減少できることが分かる。
【００４９】
次に、図１４Ａに図７Ｂに示した第１の分岐なしの経路に対する先頭追従制御の応答を示し、図１４Ｂに図７Ｃに示した第２の分岐なしの経路に対する先頭追従制御の応答を示す。実験による湾曲形状と運動学モデルによる湾曲形状の誤差応答を示している。なお、本節では運動学モデル補正の検証を行っているため、先頭追従制御には補正行列Γ-1による補償を行っていない。本実施形態では、基台変位zbにおける、実験による湾曲形状と運動学モデルによる形状の運動学的誤差を、下記の式（３１）のように各湾曲区間の遠位端変位の誤差の総和と定義する。
【数１１】
2019532824.tif 000013
【００５０】
拡張最急降下法により得られる補正行列Γを用いる運動学モデルによる誤差を実線で、比較として補正を行わない運動学的誤差を点線で示す。図１４Ａおよび図１４Ｂの双方において、補正行列Γによる応答は、補正を行わない運動学モデルによる応答と比較して、先頭追従制御の基台変位zb=０.０２〜０.０６mの区間において、運動学的誤差を低減している。また、経路の終点での姿勢を代表形状として選んでいるため、基台変位が進むにつれて誤差低減の割合が高くなっていることが分かる。本実施形態では、先頭追従制御では、一般的に経路の終点での精度が重要視されるために、経路の終点での姿勢を代表形状として選んでいる。このように、二分岐を有する経路に対する先頭追従制御では、１２０通りの湾曲目標角度が生成されるが、その中からわずか２種類の代表形状を選ぶことで全経路の約８０％の区間において、運動学的誤差を低減可能であることが分かる。また、経路の中間での運動学的誤差を重視して低減するには、任意の基台座標における湾曲姿勢を代表形状として追加すればよい。これより、本実施形態による拡張最急降下法による補正係数最適化アルゴリズムが先端追従制御を伴う運動学モデル補正に対して有効であることが分かる。
【００５１】
本実施形態によれば、分岐を有する経路に略沿って進ませるべき連続体ロボットの湾曲可能部の目標姿勢のデータを与えたときに取得されるアクチュエータの駆動量を補正することにより、連続体ロボットの湾曲可能部の目標姿勢に対する誤差を低減できる。具体的には、分岐を有する経路の中から数か所を選び、連続体ロボットの運動学モデルを補正するモデルの係数を、最適化アルゴリズムを用いて算出する。そして、その補正係数を用いて、アクチュエータ駆動量に係わる制御系を改善することにより、目標とする経路からの誤差を低減することが可能となる。このアルゴリズムは、経路の一部分を用いることで、経路全体における誤差を低減することが可能であるため、経路誤差の補正に必要とされる時間や記憶容量が低減される。
【００５２】
（第２の実施形態）
第２の実施形態では、空間を有する経路に対して先頭追従制御の制御性能向上を行う。経路に空間があるとき、空間内の壁面の観察や操作を行うことがある。このとき、空間内でとりうるロボットの湾曲形状は様々であり、例えば湾曲角度の補正テーブルを全ての姿勢に対して用意しておくことはデータが膨大となり現実的ではない。また、観察のような操作は、操作者により実時間で行われることが多く、テーブル補正では対応できない。そこで、第２の実施形態では、第１の実施形態の図８で示した最適化手法を、空間を有する経路に対して適用する。
【００５３】
図１５Ａ〜図１５Ｄに空間を有する経路を示す。経路を点線で示し、空間を破線で示している。本実施形態では、最遠位端の湾曲区間を屈曲させることで、空間内の壁面を観察するものとする。そこで、図１５Ａにそれぞれ実線および一点鎖線で示す最遠位端の湾曲角度を空間内の壁面に接触する直前まで増加および減少させる姿勢を拡張最急降下法の代表姿勢とする。次に、図１５Ｂに示すように、最遠位端一つ手前の湾曲区間全体が空間内に侵入する基台変位において、実線および一点鎖線で示す姿勢を代表姿勢として選ぶ。さらに、この手順を、図１５Ｃを経て、図１５Ｄに示すように、ロボットが経路の終点に到達するまで繰り返す。この手順を図１６にフローチャートで示す。
【００５４】
第１の実施形態と同様に、得られる代表形状を用いて拡張最急降下法による最適化を行い、補正行列Γを得ることができる。この補正行列を用いて制御を行うことにより、先端追従制御を行いながら、空間内の壁面の観察や操作を行うための湾曲形状に対して制御精度を向上させることが可能である。また本実施形態では、図８において、抽出器ブロックが図１６のフローチャートによる手順に相当し、Tra1,...,Trabがこの手順により生成される、空間内でのb通りの最遠位端の湾曲形状に相当する。
【００５５】
（第３の実施形態）
（１．運動学モデル補正）
【００５６】
第１の実施形態では分岐なし経路のそれぞれの終点における形状を代表形状（目標姿勢）として選び、補正行列Γの最適化を行った。そこでは、わずか２種類の代表形状を選ぶことのみにより、全経路の約８０％の区間において、運動学的誤差を低減した。第３の実施形態では、最適化に要する演算量は増加するものの、経路のより広い範囲にわたり湾曲可能部の湾曲形状の運動学的誤差を低減するための補正行列Γの最適化法を説明する。以下、図１７を参照しながらステップに分けて説明する。
【００５７】
（ステップ１）
２.１節〜２.３節に示した手順に従い、ｂ本の分岐なしの経路に対して先頭追従制御を行うための、ｂ個の湾曲目標角度行列Θrefp(p=1,…,b)を算出する。
【００５８】
（ステップ２）
基台部が第ζの経路評価点Pζにあるときの、湾曲目標角度θrefζを制御系に与える。３章と同様の手順により、基台変位zbが経路評価点P1,...,Pcにあるときの湾曲形状を実験により取得する。そして、補正行列Γを用いる前の運動学モデルによる湾曲形状に対する実験による湾曲形状の誤差を式(３１)により求め、運動学的誤差Eki(zb)[0]とする。
【００５９】
（ステップ３）
各分岐なし経路において最初の代表形状を選び、１．３節に示した補正行列Γの最適化を行う。ここで、代表形状としてそれぞれの経路の終点における姿勢を選ぶと、補正行列Γは第１の実施形態と同様となる。第１の実施形態では、代表形状は各分岐なし経路においてそれぞれ１種類であったが、本実施形態では、ステップ４および５をq回反復することで、各経路に対し代表形状を複数追加し、補正行列Γを再最適化する。ただし、少なくとも１つの経路について、代表形状を少なくとも１つ追加して、補正行列Γを再最適化してもよい。このようにして得られる補正行列Γを、第q回の代表形状追加による補正行列Γとよぶ。
【００６０】
（ステップ４）
前ステップで求められた補正行列Γにより補正された運動学モデルによる湾曲形状と、実験による湾曲形状との第q回目の運動学的誤差Eki(zb)[q]を式(３１)により求める。そして、運動学的誤差の収束判断を行う。いずれかの分岐なし経路において、次式に示す所定の条件を満たさず運動学的誤差が第q-1回目に比べ増加している場合は、繰り返しを終了する。
||Eki(zb) [q]|| < ||Eki(zb) [q-1]|| （３２）
式（３２）を満たして、増加していなければステップ５に進む。
【００６１】
（ステップ５）
各分岐なし経路において、第q回目の運動学的誤差Eki(zb) [q]が最大となる基台変位zb[q]を求め、基台変位zb[q]における実験による湾曲形状を代表形状として加える。そして、１．３節に示した補正行列Γの最適化を行い、ステップ４に戻る。
【００６２】
本実施形態では、上記のステップ４において、式（３２）に示すように、繰り返しの終了の評価に運動学的誤差のノルムを用いているが、それ以外に運動学的誤差の平均値や最大値や最小値、もしくは経路終端の変位などを用いてもよい。
【００６３】
（２．実験）
第１の実施形態と同様に、分岐を含む経路は図７Ａとし、第１の分岐なし経路および第２の分岐なし経路を図７Ｂおよび図７Ｃに示すようにそれぞれ抽出する。図１８Ａおよび図１８Ｂに、それぞれ図７Ｂおよび図７Ｃに示した第１、第２の分岐なしの経路に対する先頭追従制御における、実験による湾曲形状と運動学モデルによる湾曲形状の誤差応答を示している。第１、２、３回の代表形状追加による補正行列Γを用いる運動学モデルによる誤差（代表形状追加による誤差とも略す）を破線、一点鎖線、実線で示し、比較として補正を行わない運動学的誤差を点線で示す。図１８Ａに破線で示す第１の分岐なし経路では、基台変位zb＝０.０４７ｍにおいて運動学的誤差が最大となり、図１８Ｂに破線で示す第２の分岐なし経路では、基台変位zb＝０.０４６ｍにおいて運動学的誤差が最大となる。そこで、この運動学的誤差が最大となる変位における湾曲形状を代表形状として追加し、第２回の代表形状追加による補正行列Γの最適化を行う。第２回の代表形状追加による誤差応答を図１８Ａおよび図１８Ｂに一点鎖線で示す。代表形状を追加し補正行列Γを再最適化することにより、運動学的誤差は経路全域にわたり大幅に低減されていることがわかる。さらに、第１の分岐なし経路において、基台変位zbが０.０３９ｍにおける湾曲形状を代表形状として追加し、第２の分岐なし経路において、基台変位zbが０.０３７ｍにおける湾曲形状を代表形状として追加し、第３回の代表形状追加による補正行列Γの最適化を行う。第３回の代表形状追加による誤差応答を図１８Ａおよび図１８Ｂに実線で示す。これより、運動学的誤差は、特に０.０３ｍから０.０４ｍにおいて、さらに低減されていることがわかる。
【００６４】
さらに、第３回の代表形状追加による誤差応答を用いて、第４回の代表形状追加による補正行列Γの再最適化を行う。運動学的応答を図１９Ａおよび図１９Ｂに実線で示す。なお、比較として補正を行わない運動学的誤差を点線で、第３回の代表形状追加による誤差応答を破線で示している。図１９Ａに示す第１の分岐なし経路ではさらに誤差を低減している。しかし、図１９Ｂに示す第２の分岐なし経路の誤差応答より、基台変位zbが０.０５２ｍ付近において、第４回の代表形状追加による誤差は、第３回目に比べて誤差が増加している。代表形状追加回数に対する運動学的誤差を評価するため、運動学的誤差のノルムを求める。図２０に、第１の分岐なし経路について実線で運動学的誤差のノルムを示し、第２の分岐なし経路について破線で運動学的誤差のノルムを示す。第２の分岐なし経路の運動学的誤差は、第４回目の代表形状追加で増加しており、式（３２）を満たしていないことがわかる。そこで、補正行列Γの更新を終了し、第３回の代表形状追加による補正行列Γを補正行列として採用することができる。
【００６５】
上述のとおり、本実施形態で説明した手法を用いれば、運動学的誤差を低減したい連続体ロボットの姿勢を代表形状として採用することにより、第１および第２の実施形態の手法よりも広い範囲にわたって湾曲可能部の湾曲形状の運動学的誤差を低減できる。
【００６６】
（その他の実施形態）
上述から理解できるように、制御方法は、ロボットがロボットの評価点に対応するc個に等間隔で分割されると仮定して説明した。また、経路全体が2c個に等間隔で分割されると仮定した。経路の区間数がロボットの評価点数の倍数（上記の例では２）である場合、ロボットの運動学的モデルの計算負荷を軽減するのに有利である。ｃの長さを評価ステップ毎にロボットが移動する距離になるように設定すれば、計算負荷を減らすのにも有益である。
さらに、マウス、タッチスクリーンなどの入力手段をシステムに設けて、システムのユーザがロボットにより追従すべき経路を識別することが可能になる。一例では、システムは、表示部（ＬＣＤ、ＣＲＴなど）に画像を表示させる表示コントローラをさらに含み、その上でユーザは経路を識別することができる。例えば、ユーザは、経路を表す線を引いたり、ロボットがたどるべき複数の点を入力したりすることができる。後者の場合、コントローラは、経路を表す線を形成するために入力点の補間を実行し、その後、上述したように経路を複数の区間に分割することができる。表示部に提供される画像は、カメラ、医療機器（ＭＲＩ、ＣＴなど）、音響（例えば超音波）検査装置などを使用して撮影された画像でもよい。上述の実施形態は、１つの実施形態の態様を別の実施形態の態様と組み合わせ、その組み合わせを本発明の思想の範囲内で変更してもよい。
本発明の実施形態はまた、（より完全には「固定コンピュータ可読記憶媒体」とも称され得る）記憶媒体上に記録されたコンピュータ実行可能命令（たとえば１つまたは複数のプログラム）を読み出し、実行して、上記で説明した実施形態の１つまたは複数の機能を実行する、および／または上記で説明した実施形態の１つまたは複数の機能を実施するための１つまたは複数の回路（たとえば、特定用途向け集積回路（ＡＳＩＣ））を含む、システムまたは装置のコンピュータによって、また、システムまたは装置のコンピュータにより、たとえば、コンピュータ実行可能命令を記憶媒体から読み出し、実行して上記で説明した実施形態の１つまたは複数の機能を実施し、および／または１つまたは複数の回路を制御して上記で説明した実施形態の１つまたは複数の機能を実施することによって実施される方法によって実現され得る。コンピュータは、１つまたは複数のプロセッサ（たとえば中央処理ユニット（ＣＰＵ）、マイクロ処理ユニット（ＭＰＵ））を備えることができ、別個のコンピュータまたは別個のプロセッサのネットワークを含んで、コンピュータ実行可能命令を読み出し、実行することができる。コンピュータ実行可能命令は、たとえば、ネットワークまたは記憶媒体からコンピュータに提供することができる。記憶媒体は、たとえば、ハードディスク、ランダムアクセスメモリ（ＲＡＭ）、読み取り専用メモリ（ＲＯＭ）、分散コンピューティングシステムの記憶装置、光学ディスク（コンパクトディスク（ＣＤ）、デジタル多目的ディスク（ＤＶＤ）、またはブルーレイディスク（ＢＤ）（商標）など）、フラッシュメモリデバイス、メモリーカードなどを含むことができる。
【００６７】
例示的な実施形態を参照して本発明を説明してきたが、本発明は開示された例示的な実施形態に限定されないことを理解されたい。特許請求の範囲は、最も広範な解釈に従うものとし、それによってすべてのそのような改変形態ならびに等価の構造および機能を包含する。
【００６８】
本願は、２０１６年８月２２日出願の日本特許出願第２０１６−１６２２９７号および２０１７年４月２９日出願の日本特許出願第２０１７−０９０８８２号の利益を主張するものであり、参照によってそれらは全体として本明細書に組み込まれる。
【符号の説明】
【００６９】
１００  連続体ロボット
１０１〜１０６  湾曲区間（第１の湾曲可能部、第２の湾曲可能部）
１１１〜１１６  ワイヤ（第１のワイヤ、第２のワイヤ）
１３１〜１３６  アクチュエータ（制御部）
【図１】
2019532824.tif 000014
【図２】
2019532824.tif 000015
【図３】
2019532824.tif 000016
【図４】
2019532824.tif 000017
【図５】
2019532824.tif 000018
【図６】
2019532824.tif 000019
【図７Ａ】
2019532824.tif 000020
【図７Ｂ】
2019532824.tif 000021
【図７Ｃ】
2019532824.tif 000022
【図８】
2019532824.tif 000023
【図９】
2019532824.tif 000024
【図１０】
2019532824.tif 000025
【図１１】
2019532824.tif 000026
【図１２】
2019532824.tif 000027
【図１３Ａ】
2019532824.tif 000028
【図１３Ｂ】
2019532824.tif 000029
【図１４Ａ】
2019532824.tif 000030
【図１４Ｂ】
2019532824.tif 000031
【図１５Ａ】
2019532824.tif 000032
【図１５Ｂ】
2019532824.tif 000033
【図１５Ｃ】
2019532824.tif 000034
【図１５Ｄ】
2019532824.tif 000035
【図１６】
2019532824.tif 000036
【図１７】
2019532824.tif 000037
【図１８Ａ】
2019532824.tif 000038
【図１８Ｂ】
2019532824.tif 000039
【図１９Ａ】
2019532824.tif 000040
【図１９Ｂ】
2019532824.tif 000041
【図２０】
2019532824.tif 000042
【国際調査報告】
2019532824.tif 000043
2019532824.tif 000044