(57)【要約】【課題】連続体ロボットの湾曲可能部の目標姿勢に対する誤差を低減することができる技術を提供する。【解決手段】連続体ロボット１００は、湾曲可能な第１の湾曲可能部１０１〜１０６と、第１の湾曲可能部と隣接して設けられた湾曲可能な第２の湾曲可能部と、第１の湾曲可能部に接続された第１のワイヤ１１１〜１１６と、第２の湾曲可能部に接続された第２のワイヤと、第１および第２のワイヤの駆動を制御して第１および第２の湾曲可能部の湾曲を制御する制御部１１１〜１１６を有する。制御部は、第１のワイヤを駆動することに伴う第２の湾曲可能部の湾曲および第２のワイヤを駆動することに伴う第１の湾曲可能部の湾曲を考慮した運動学に基づいて、第１および第２のワイヤの駆動を制御する。或いは、制御部は、第１の湾曲可能部の湾曲目標値を、第１および第２の湾曲可能部の湾曲量の和によって達成するように第１および第２のワイヤの駆動を制御する。【選択図】図１
2018008335.tif 000002
【特許請求の範囲】
【請求項１】
湾曲可能に構成された第１の湾曲可能部と、
前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、
前記第１の湾曲可能部に接続された第１のワイヤと、
前記第２の湾曲可能部に接続された第２のワイヤと、
前記第１および第２のワイヤの駆動を制御することにより、前記第１および第２の湾曲可能部の湾曲を制御する制御部と、を有し、
前記制御部は、
前記第１の湾曲可能部を湾曲させるべく前記第１のワイヤを駆動することに伴う前記第２の湾曲可能部の湾曲および前記第２の湾曲可能部を湾曲させるべく前記第２のワイヤを駆動することに伴う前記第１の湾曲可能部の湾曲を考慮した運動学に基づいて、前記第１および第２のワイヤの駆動を制御する、
ことを特徴とする連続体ロボット。
【請求項２】
湾曲可能に構成された第１の湾曲可能部と、
前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、
前記第１の湾曲可能部に接続された第１のワイヤと、
前記第２の湾曲可能部に接続された第２のワイヤと、
前記第１および第２のワイヤの駆動を制御することにより、前記第１および第２の湾曲可能部の湾曲を制御する制御部と、を有し、
前記制御部は、
前記第１の湾曲可能部の湾曲目標値を、前記第１および第２の湾曲可能部のそれぞれの湾曲量の和によって達成するように前記第１および第２のワイヤの駆動を制御する、
ことを特徴とする連続体ロボット。
【請求項３】
前記制御部は、先頭追従制御を行う、
ことを特徴とする請求項１または２に記載の連続体ロボット。
【請求項４】
湾曲可能に構成された第１の湾曲可能部と、前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、前記第１の湾曲可能部に接続された第１のワイヤと、前記第２の湾曲可能部に接続された第２のワイヤと、を含む連続体ロボットの運動学の補正方法であって
前記第１の湾曲可能部を湾曲させるべく前記第１のワイヤを駆動することに伴う前記第２の湾曲可能部の湾曲および前記第２の湾曲可能部を湾曲させるべく前記第２のワイヤを駆動することに伴う前記第１の湾曲可能部の湾曲を考慮したモデルを用いて運動学を補正する、
ことを特徴とする補正方法。
【請求項５】
湾曲可能に構成された第１の湾曲可能部と、前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、前記第１の湾曲可能部に接続された第１のワイヤと、前記第２の湾曲可能部に接続された第２のワイヤと、を含む連続体ロボットの運動学の補正方法であって
前記第１の湾曲可能部の湾曲目標値を、前記第１および第２の湾曲可能部のそれぞれの湾曲量の和によって達成するように前記第１および第２のワイヤの駆動を制御するモデルを用いて運動学を補正する、
ことを特徴とする補正方法。
【請求項６】
前記湾曲可能部の目標姿勢のデータと、前記連続体ロボットの機構に基づいて前記データから導出される前記第１および第２のワイヤの駆動変位による前記湾曲可能部の姿勢と、の関係を呈示する運動学を補正するための補正値を、前記連続体ロボットに所定の姿勢を取らせるときの前記湾曲可能部の目標姿勢のデータと前記湾曲可能部の実際の姿勢に係わる測定値との誤差を減少すべく最適化手法を用いるアルゴリズムにより求める、
ことを特徴とする請求項４または５に記載の補正方法。
【請求項７】
前記連続体ロボットの第一の所定の代表姿勢のデータに基づく前記ワイヤの駆動変位により制御された前記湾曲可能部の測定変位を取得し、前記測定変位と前記第一の所定の代表姿勢のデータと補正係数の初期値とを用いて、最急降下法により前記第一の所定の代表姿勢に対する補正係数を求め、この手法を、第二の所定の代表姿勢に対しても同様に用いて前記第二の所定の代表姿勢に対する補正係数を求めることで、複数の所定の代表姿勢に対してそれぞれ補正係数を取得して、前記補正値を求める、
ことを特徴とする請求項６に記載の補正方法。
【請求項８】
前記第二の所定の代表姿勢に対する最急降下法の補正係数の初期値に、前記第一の所定の代表姿勢に対して得られた補正係数を用いる、
ことを特徴とする請求項７に記載の補正方法。
【請求項９】
前記最適化手法を反復的に用い、各回の補正係数の初期値に、一つ前の回で得られる補正係数を用いる、
ことを特徴とする請求項７または８に記載の補正方法。
【請求項１０】
全ての前記補正係数の平均値を求め、これを前記補正値とする、
ことを特徴とする請求項７から９の何れか１項に記載の補正方法。
【請求項１１】
第iの湾曲可能部の湾曲相対角度に係数γp[i]を乗じた角度を、隣接する第i-１の湾曲可能部の湾曲相対角度に加算し、前記第iの湾曲可能部の湾曲相対角度に別の係数γd[i]を乗じた角度を、隣接する第i+１の湾曲可能部の湾曲相対角度に加算し、前記第iの湾曲可能部の湾曲相対角度からは、これに（γp[i]+γd[i]）を乗じた角度を減算するモデルを用いて前記補正値を求める（ただし、iは２以上で、前記湾曲可能部の数より１少ない数以下）、
ことを特徴とする請求項６から１０の何れか１項に記載の補正方法。
【請求項１２】
前記補正値は補正行列である、
ことを特徴とする請求項１１に記載の補正方法。
【請求項１３】
前記補正行列では、
i行i列を１-γd[i]-γp[i]とし、i行i-１列をγd[i-１]とし、i行i+１列をγp[i+１]とする、
ことを特徴とする請求項１２に記載の補正方法。
【請求項１４】
湾曲可能に構成された第１の湾曲可能部と、前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、前記第１の湾曲可能部に接続された第１のワイヤと、前記第２の湾曲可能部に接続された第２のワイヤと、を含む連続体ロボットの制御方法であって
請求項６に記載の補正方法を利用し、前記補正値の逆値を前記湾曲可能部の目標姿勢のデータに乗じて補正された目標姿勢を求め、前記補正された目標姿勢に基づいて演算される前記ワイヤの駆動変位により前記湾曲可動部を制御する、
ことを特徴とする制御方法。
【請求項１５】
湾曲形状の追加目標値を前記目標姿勢のデータに加算することにより、予め設定された湾曲形状のみではなく、実時間の操作による湾曲形状の制御を行う、
ことを特徴とする請求項１４に記載の制御方法。
【請求項１６】
前記補正値の逆値に追加ゲインを乗ずることにより、曲率を一定としない経路に対する先頭追従制御を行う、
ことを特徴とする請求項１４または１５に記載の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、連続体ロボット、その運動学の補正方法、および連続体ロボットの制御方法に関する。
【背景技術】
【０００２】
連続体ロボットはコンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ複数の湾曲区間（湾曲可能部）からなり、その湾曲区間を変形ないし移動させることにより全体の形状が制御される。このロボットは、剛体リンクにより構成されるロボットに対して、主に二つの優位性を持つ。一つ目は、剛体リンクロボットが嵌まり込んでしまうような、狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。二つ目は、連続体ロボットは本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。この特徴を生かし、内視鏡のシースやカテーテルなど医療の分野のロボットや、レスキューロボットなど極限作業ロボットへの応用が期待されている。しかし、連続体ロボットはその柔軟構造により、自由度が無限大となるため、運動学の導出は難しい。すなわち、湾曲区間の目標姿勢のデータを与えたときに、アクチュエータの駆動量を適切に演算して湾曲区間をどの様に駆動すれば前記目標姿勢が実現されるかを提示するプロセスの導出は容易ではない。
【０００３】
そこで、非特許文献１では、湾曲区間を区分的に曲率を一定と仮定する運動学の導出が紹介されている。この方法は、多くの連続体ロボットに適用されている。この運動学を用いて、形状制御のためのアクチュエータ駆動量を算出することも可能である。さらに、特許文献１に開示された技術では、運動学の導出に加え、その誤差を補正するため、湾曲区間先端に変位および角度センサを設けて湾曲区間のフィードバック制御を行うことにより、位置決め制御の高性能化を図っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１３/０３００５３７号明細書
【非特許文献】
【０００５】
【非特許文献１】Design and Kinematic Modeling of Constant Curvature Continuum Robots:A Review, Robert J. Webster III and Bryan A.Jones, Thc international Joumal of Robotics Research ２９（１３）１６６１-１６８３
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし、非特許文献１の運動学による手法は、摩擦やロボットのねじれ、さらにはアクチェータとして用いるワイヤの伸縮などのモデル化誤差を考慮しておらず、その誤差により形状制御の精度が劣化してしまうことがある。特許文献１では、磁気センサを用いて３つの湾曲区間の位置を補償しているが、細径化される連続体ロボットには磁気センサの設置が難しいという問題がある。また、磁気センサは連続体ロボットの外部に検出系を設置する必要があり、連続体ロボットの用途を限定してしまう恐れがある。
【課題を解決するための手段】
【０００７】
本発明の一側面の連続体ロボットは、湾曲可能に構成された第１の湾曲可能部と、前記第１の湾曲可能部と隣接して設けられるとともに、湾曲可能に構成された少なくとも１つの第２の湾曲可能部と、前記第１の湾曲可能部に接続された第１のワイヤと、前記第２の湾曲可能部に接続された第２のワイヤと、前記第１および第２のワイヤの駆動を制御することにより、前記第１および第２の湾曲可能部の湾曲を制御する制御部と、を有し、前記制御部は、前記第１の湾曲可能部を湾曲させるべく前記第１のワイヤを駆動することに伴う前記第２の湾曲可能部の湾曲および前記第２の湾曲可能部を湾曲させるべく前記第２のワイヤを駆動することに伴う前記第１の湾曲可能部の湾曲を考慮した運動学に基づいて、前記第１および第２のワイヤの駆動を制御する。或いは、前記制御部は、前記第１の湾曲可能部の湾曲目標値を、前記第１および第２の湾曲可能部のそれぞれの湾曲量の和によって達成するように前記第１および第２のワイヤの駆動を制御する。
【０００８】
また、本発明の他の側面の連続体ロボットの運動学の補正方法は、前記第１の湾曲可能部を湾曲させるべく前記第１のワイヤを駆動することに伴う前記第２の湾曲可能部の湾曲および前記第２の湾曲可能部を湾曲させるべく前記第２のワイヤを駆動することに伴う前記第１の湾曲可能部の湾曲を考慮したモデルを用いて運動学を補正する。或いは、前記第１の湾曲可能部の湾曲目標値を、前記第１および第２の湾曲可能部のそれぞれの湾曲量の和によって達成するように前記第１および第２のワイヤの駆動を制御するモデルを用いて運動学を補正する。
【０００９】
また、本発明の他の側面の連続体ロボットの制御方法は、前記湾曲可能部の目標姿勢のデータと、前記連続体ロボットの機構に基づいて前記データから導出される前記第１および第２のワイヤの駆動変位による前記湾曲可能部の姿勢と、の関係を呈示する運動学を補正するための補正値を、前記連続体ロボットに所定の湾曲形状を取らせるときの前記湾曲可能部の目標姿勢のデータと前記湾曲可能部の実際の姿勢に係わる測定値との誤差を減少すべく最適化手法を用いるアルゴリズムにより求める補正方法を利用し、前記補正値の逆値を前記湾曲可能部の目標姿勢のデータに乗じて補正された目標姿勢を求め、前記補正された目標姿勢に基づいて演算される前記ワイヤの駆動変位により前記湾曲可動部を制御する。
【発明の効果】
【００１０】
本発明の一側面によれば、連続体ロボットの湾曲可能部の目標姿勢のデータを与えたときに取得されるアクチュエータの駆動量を補正することにより、連続体ロボットの湾曲可能部の目標姿勢に対する誤差を低減することができる。
【図面の簡単な説明】
【００１１】
【図１】実施例１における装置を示す図である。
【図２】実施例１における運動学モデルを示す図である。
【図３】実施例１における運動学モデルを示す図である。
【図４】実施例１における運動学モデルを示す図である。
【図５】実施例１における最適化アルゴリズムを示すブロック線図である。
【図６】実施例１における制御系を示すブロック線図である。
【図７】実施例１における変位測定装置を示す図である。
【図８】実施例１における実験結果を示す図である。
【図９】実施例１における実験結果を示す図である。
【図１０】実施例１における実験結果を示す図である。
【図１１】実施例１における実験結果を示す図である。
【図１２】実施例２における先頭追従制御を示す図である。
【図１３】実施例２における運動学モデルを示す図である。
【図１４】実施例２における制御系を示すブロック線図である。
【図１５】実施例２における経路と機構的誤差を示すブロック線図である。
【図１６】実施例２における実験結果を示す図である。
【図１７】実施例２における制御指令を示す図である。
【図１８】実施例２における実験結果を示す図である。
【図１９】実施例２における実験結果を示す図である。
【図２０】実施例３における制御系を示すブロック線図である。
【図２１】実施例３における経路と機構的誤差を示すブロック線図である。
【図２２】実施例３における最適化アルゴリズムを示す図である。
【図２３】実施例２における実験結果を示す図である。
【図２４】実施例２における実験結果を示す図である。
【発明を実施するための形態】
【００１２】
本発明では、湾曲可能部間の連成を考慮したモデルを用いて運動学を補正し、その補正された運動学に基づいて、連続体ロボットを構成したり、連続体ロボットの制御方法を構築したりすることができる。制御方法では、湾曲可能部の目標姿勢のデータ（湾曲目標値）と前記補正された運動学における補正値の逆値（補正値との積が１となる値）とを用いて、前記目標姿勢を達成するアクチュエータの駆動量を取得して、湾曲可動部を制御する。湾曲形状の追加目標値を、逐次、前記湾曲目標値に加算することにより、予め設定された湾曲量のみではなく、実時間の操作による湾曲形状の実時間制御を行うこともできる。これを利用して先頭追従制御を行うことができ、前記補正値の逆値に追加ゲインを乗ずることにより、曲率を一定としない経路に対する先頭追従制御を行うこともできる。
【００１３】
（実施例１）
実施例１では、湾曲可能部である湾曲区間を区分的に曲率を一定と仮定する運動学に加えて、複数の機構的要因により生ずる隣接湾曲区間の間の連成のモデルを導入して前記運動学を補正する方法を示す。そして、それによる連続体ロボットの駆動・制御を向上させる手法を示す。連続体ロボットの運動学は、湾曲区間の目標姿勢のデータを与えたときに、連続体ロボットの機構に基づいて該データから導かれるアクチュエータ（駆動部）の駆動量により如何なる湾曲区間の姿勢が実現されるかを呈示する。
【００１４】
その運動学の補正は、連続体ロボットの湾曲形状を実験的に測定し、最適化手法を反復的に用いるアルゴリズムにより前記運動学を補正するための係数（補正値）を求めることで行われる。運動学補正係数からは、運動学補正行列を求め、その逆行列を用いることで、連続体ロボットの制御系が実際に実現する湾曲形状の目標形状に対する誤差を低減することができる。
【００１５】
図５に、本実施例で用いる運動学補正係数ベクトルγを求める最適化アルゴリズムのブロック線図を示す。ここで、Pは連続体ロボットを表し、Kは、各湾曲区間の遠位端の角度を目標値としてワイヤ駆動変位量を求めるブロックである。まず、所定の代表姿勢である第一の代表形状ベクトルθrep[１]を設定し、ワイヤ変位演算ブロックKにより、ワイヤ駆動変位lp[１]を求める。次に、ロボットPに対して、このワイヤ駆動変位を与えて制御し、図７に示す湾曲区間遠位端の変位測定系により、制御されたロボットの湾曲形状の測定変位xex[１],zex[１]を取得する。この測定変位と代表形状ベクトルθrep[１]と補正係数の初期ベクトルγ０とを用いて、最急降下法演算ブロックSDMにより第一の代表形状に対する補正係数ベクトルγ[１]１が求まる。次に、第二の代表形状に対して、同様に補正係数γ[２]１を求めるが、本実施例では、第二の代表形状に対する最急降下法の初期値に、第一の代表形状に対して得られた補正係数γ[１]１を用いる。これにより、補正係数ベクトルが局所的最小値となることを回避することができる。
【００１６】
そして、a通りの代表形状に対してそれぞれ補正係数γ[a]１を求める。ここまでが第一回目の反復試行となる。次に第二回目の反復試行を行うが、その補正係数の初期値は第一回目の反復試行で得られる各代表形状に対する補正係数γ[a]１とする。こうして、第一回目の試行と同様に第二回目の試行が行われる。この試行をh回反復し、補正係数が十分に収束したかをブロックCJBによって判断する。さらに、全ての補正係数の平均値を平均値演算ブロック1/(ah)により求め、これを補正係数ベクトルγとする。平均値は、全ての補正係数を合成することで得られる他のものに置き置えてもよい。例えば、中央値、代表形状の態様に応じて決められた重みを考慮した重み付きの平均値、などでもよい。結果的に充分な補正効果が得られるものであれば、どの様な合成値でもよい。前記補正係数γ[a]１などは、複数の代表形状（姿勢）のうちの少なくとも１つにおける補正結果を用いて別の代表形状（姿勢）における前記運動学を補正する場合の初期値として利用することができる。
【００１７】
以下に、連続体ロボットの運動学の導出と補正係数最適化アルゴリズム、さらには制御系について詳細に記述し、実験によって得られた制御結果を示す。
【００１８】
１.１）連続体ロボットのモデリング
１.１.１）運動学の導出
図１に、本実施例で用いる６つの湾曲区間１０１〜１０６を含む連続体ロボット１００の概略図を示す。連続体ロボットは各湾曲区間の遠位端１２１〜１２６に駆動用のワイヤ１１１〜１１６がそれぞれ接続され、それらのワイヤを、ロボット基台部１４０に設置されたアクチュエータ１３１〜１３６でそれぞれ押し引きすることにより湾曲区間の姿勢が制御される。図１に示す例では、駆動用ワイヤが湾曲区間の遠位端に接続される位置が、中心軸を挟んで交互に反対側に移っているが、ワイヤの湾曲区間への接続位置はこの形態に限らない。ワイヤの押し引きにより湾曲区間の姿勢が制御できれば接続箇所の位置は問わない。基台部１４０はz軸方向に自由度を持ち、その移動変位が検出可能である。図２に第１湾曲区間１０１を取り上げ、湾曲区間の筐体とワイヤ１１１からなる構造の模式図を示す。図１と図２において、筐体の中心軸は破線ないし一点鎖線で示す。連続体ロボットは、不図示の制御部をさらに有し、制御部は、目標姿勢が入力されると、各湾曲区間によりその目標姿勢を達成するように、アクチュエータによるワイヤの駆動を制御する信号を出力する。つまり、制御部は、後述する運動学に基づいて第１および第２のワイヤの駆動を制御する。目標姿勢の入力は、ユーザが行っても良いし、不図示の目標姿勢入力部が、あらかじめ記憶されたプログラムに従って行っても良い。目標姿勢は、湾曲区間ごとの角度として入力されてもよい。制御部は、たとえばコンピュータにおける１つ以上のプロセッサがプログラムを読み出して実行するものであってもよいし、専用に設けられた回路（ＦＰＧＡやＡＳＩＣ）として実現されても良い。
【００１９】
以下の説明における記号の定義を次に示す。ln：第n湾曲区間のアーム筐体の長さ、rn：第n湾曲区間のアーム筐体の中心軸からワイヤまでの変位、e：ロボットの湾曲区間数、θn：第n湾曲区間の遠位端の角度、ρn：第n湾曲区間の曲率半径、θrefn：第n湾曲区間の遠位端の目標角度、lpn：第n湾曲区間のワイヤの駆動変位、xtn,ztn：第n湾曲区間の遠位端の座標、c：ロボットの評価点数、xi,zi：ロボットを長手方向にc分割するときの第i番目の座標、zb：基台部の変位。
【００２０】
次のような仮定をおき、図３（図４も参照）に示す湾曲区間数をnとする連続体ロボットの運動学を導出する。
１．筐体およびワイヤは紙面内のみに変形する。
２．各湾曲区間において、筐体およびワイヤは曲率一定に変形する。
３．筐体およびワイヤのねじり変形は考慮しない。
４．筐体およびワイヤは長手方向に変形しない。
５．筐体にワイヤガイドが設けられ、筐体中心軸の円弧とワイヤによって作られる円弧との中心が常に等しい。
６．筐体とワイヤ間の摩擦は考慮しない。
【００２１】
まず、ワイヤ駆動変位と湾曲区間の遠位端の角度との関係を導出する。第１湾曲区間のみを考えると、ワイヤの駆動量lp１と第１湾曲区間の遠位端の角度θ１の関係は、下記の（１）式である。ここで、下記の（２）式が成り立つから、式（１）、式（２）より、下記の式（３）が得られる。次に、第n湾曲区間のワイヤ駆動変位lpnと遠位端の角度θnの関係を導出する。ただし、nは２以上とする。第n湾曲区間の湾曲相対角度である上ティルダ付きθｎを下記の式（４）のように定義する。そして、図３に示すように原点をxtn-１,ztn-１とし、θn-１方向およびその直交方向からなる相対座標系xn-znをとる。すると、相対座標系xn-znにおけるワイヤの駆動変位である上ティルダ付きlpnと第ｎ湾曲区間の遠位端の相対角度である上ティルダ付きθnの関係は、下記の式（５）である。第n湾曲区間のワイヤ駆動変位lpnは、第１〜第n-１区間までの相対座標系における第n湾曲区間を駆動するためのワイヤの変位の総和となり、下記の式（６）となる。
【００２２】
【数１−６】
2018008335.tif 000003
【００２３】
これより、第n湾曲区間の遠位端の角度θnは、ワイヤ駆動変位lpnによってのみ決まり、途中の湾曲区間の角度には依存しないことが分かる。
【００２４】
次に、第n湾曲区間の遠位端角度と遠位端座標の関係を導出する。まず、第１湾曲区間を考え、ρを曲率半径とすると、第１湾曲区間の遠位端の座標xt１,zt１は、下記の式（７）、式（８）となる。ここで、上記の式（２）を式（７）、式（８）に代入すると、下記の式（９）、式（１０）が得られる。ここで、第n湾曲区間の遠位端角度と遠位端座標の関係を導出する。ただし、nは２以上とする。相対座標系xn-znにおける湾曲区間の遠位端の座標である上ティルダ付きxtn、上ティルダ付ztnは下記の式（１１）、式（１２）となる。
【００２５】
これより、絶対座標系での遠位端の座標xtn,ztnは回転変換行列を用いて、下記の式（１３）となる。また次項において、ロボット全体をα倍に細分割する座標を、最適化アルゴリズムの評価点として用いる。このとき、評価点の総数はc=αeであり、第iの評価点の座標xi,ziは、下記の式（１４）と求まる。ただし、Q、Rは商と剰余であり、Q=[i/α]、R=i mod αから求まる。
【００２６】
【数７−１４】
2018008335.tif 000004
【００２７】
１.１.２）隣接区間連成モデルによる運動学補正
前項では、仮定１〜５をおき運動学の導出を行ったが、ロボットの曲げ方向には柔軟で長手方向には高剛性となる筐体およびワイヤは構造設計が難しく、仮定３、４は厳密には成立しにくい。そのため、ワイヤ駆動変位に対する湾曲角度の上記関係式（６）が成立せず、制御後の実際の湾曲角度は目標角度から誤差が生じてしまう。さらに、この影響は、ワイヤを駆動する湾曲区間のみにとどまらず、他の区間の湾曲角度に誤差を生じさせ、連成問題の原因となる。そこで、本実施例では、湾曲区間の連成に対する以下の仮定を加え、運動学誤差を補正する。
【００２８】
第n湾曲区間を、湾曲角度である上ティルダ付きθｎとなるように駆動するとき、次の様な連成が生じる。すなわち、第n-1湾曲区間および第n+1湾曲区間の湾曲角度はそれぞれγｐｎ＊（上ティルダ付きθｎ）、γｄｎ＊（上ティルダ付きθｎ）増加し、第n湾曲区間の湾曲角度は（γｐｎ＋γｄｎ）＊（上ティルダ付きθｎ）減少する。ここで、γｐｎ、γｄｎは補正係数である。つまり、第iの湾曲可能部の湾曲相対角度に係数γp[i]を乗じた角度は、隣接する第i-１の湾曲可能部の湾曲相対角度に加算される。また、前記第iの湾曲可能部の湾曲相対角度に別の係数γd[i]を乗じた角度は、隣接する第i+１の湾曲可能部の湾曲相対角度に加算され、前記第iの湾曲可能部の湾曲相対角度からは、これに（γp[i]+γd[i]）を乗じた角度が減算される。こうしたモデルを用いて補正値を求める（ただし、iは２以上で、前記湾曲可能部の数より１少ない数以下）。この仮定により補正される第n湾曲区間の湾曲角度を上ティルダ付きθｇｎとすると、下記の式（１５）のように表せる。ただし、最も近位端の第１湾曲区間においては、下記の式（１６）のようになり、また、最も遠位端の第e湾曲区間においては、下記の式（１７）となる。これを行列表記すると下記の式（１８）のようになり、本実施例ではこの行列を補正行列Γと表す。補正行列Γでは、i行i列が１-γd[i]-γp[i]であり、i行i-１列がγd[i-１]であり、i行i+１列がγp[i+１]である。
【００２９】
また、運動学補正係数からなるベクトルγを下記の式（１９）のように定義し、補正係数ベクトルとよぶ。これより、補正される各湾曲区間遠位端の座標ｘｇｔｎ，ｚｇｔｎは、下記の式（２０）となる。ロボット全体をα倍に細分割するときの、第iの評価点の座標ｘｇｉ，ｚｇｉは、前記の式（１４）においてθ、上ティルダ付きθをθg、上ティルダ付きθｇと置き換えればよく、省略する。
【００３０】
【数１５−２０】
2018008335.tif 000005
【００３１】
１.１.３）補正係数最適化
前項で示した運動学補正係数を解析的に求めることは難しい。それは、湾曲角度の連成は、長手方向の圧縮・引張力により筐体およびワイヤが変形することにより生ずるが、その力は湾曲区間によってワイヤの通る本数が異なったり、筐体の剛性が湾曲角度により非線形に変動したりするためである。さらには、上記仮定１、２、５、６についても、実際は完全には成り立たず、湾曲角度の誤差の原因となる。そこで、本実施例では実験的に湾曲形状を測定し、測定値と運動学による値との誤差から、最適化の手法である最急降下法を用いて補正係数γを求める。このとき、補正係数γの最適化に好適な湾曲形状を一つ選ぶのは不可能であるため、本実施例ではa通り（aは複数）の代表形状を用いてh回（ｈは複数）の反復による最適化を行う。これを拡張最急降下法とよぶ。
【００３２】
拡張最急降下法のアルゴリズムを図５にブロック線図で示す。ブロック線図中の記号の定義を次に示す。記号[k]は第k（≦a）の代表形状を、記号jは第j（≦h）の反復試行を表す。
【００３３】
第kの代表形状ベクトルθrep[k]を下記の式（２１）と表記し、第kの代表形状に対するワイヤ駆動変位lp[k]を下記の式（２２）と表記し、ロボットの測定変位ｘｅｘ［ｋ］，ｚｅｘ［ｋ］を下記の式（２３）、式（２４）と表記する。また、SDMは、最急降下法アルゴリズムを表し、下記の式（２５）である評価関数を最小化する補正係数ベクトルγ[k]jを最適化により求めるブロックである。補正係数ベクトルγ[k]jは第kの代表形状に対する反復回数jの補正係数ベクトルである。
【００３４】
上述した様に、これらのブロックにより、拡張最急降下法により補正係数ベクトルγを得るには、まず、第一の代表形状ベクトルθrep[1]を設定し、上記式（６）の演算を行うワイヤ変位ブロックKにより、ワイヤ駆動変位lp[１]を求める。次に、ロボットPに対してワイヤ駆動変位を与えて制御し、制御されたロボットの湾曲形状の測定変位ｘｅｘ［１］，ｚｅｘ［１］を取得する。この測定変位と、代表形状θrep[１]と、補正係数の初期ベクトルγ０を用いて、最急降下法演算ブロックSDMにより第一の代表形状に対する補正係数ベクトルγ[１]１が求まる。次に、第二の代表形状に対して、同様に補正係数γ[２]１を求めるが、本実施例では、第二の代表形状に対する最急降下法の初期値に、第一の代表形状に対して得られた補正係数γ[１]１を用いる。これより、補正係数ベクトルが局所的最小値となることを回避することができる。そして、図５に示すように、a通りの代表形状に対して補正係数γ[k]１を求め、ここまでを第一回目の反復試行とする。次に第二回目の反復試行を行うが、その補正係数の初期値は第一回目の反復で得られる補正係数とする。そして、第一回目の試行と同様に第二回目の試行が行われる。この試行をh回反復し、補正係数が十分に収束したかをブロックCJBによって判断する。もしくは、反復回数は試行錯誤により予め決定されていてもよい。そして、全ての補正係数ベクトルの平均値を下記の式（２６）のように求め、これを補正係数γとする。
【００３５】
【数２１−２６】
2018008335.tif 000006
【００３６】
１.２）制御系設計
運動学補正係数は、ロボットの運動学を補正し、その湾曲形状を算出するのみではなく、湾曲形状の制御に適用可能である。第n湾曲区間に対する目標角度θrefnからなる目標湾曲形状ベクトルθrefを下記の式（２７）とし、第n湾曲区間に対する補正目標角度θmodnからなる補正湾曲形状ベクトルθmodを下記の式（２８）と定義する。
【００３７】
ロボットを目標湾曲形状に制御するには、上記補正行列Γの逆行列を用い、下記の式（２９）のように補正湾曲形状ベクトルを求める。そして、この補正目標角度を絶対座標系に変換し、上記式（６）の角度θに代入し、ワイヤ駆動変位を求める。さらに、本実施例の制御系では、湾曲形状の追加目標ベクトルθcomを目標湾曲形状ベクトルに加算することにより、予め設定された湾曲形状のみではなく、実時間の操作による湾曲形状の制御に運動学補正を適用することができる。この制御系のブロック線図を図６に示す。
【００３８】
【数２７−２９】
2018008335.tif 000007
【００３９】
１.３）実験
本節では、第２、第３節で示した運動学の補正係数γを用いる運動学補正および湾曲形状制御の有効性を示す。実験で用いるロボットのパラメタは、アーム筐体の長さl１〜l６=０.０１０m、ロボットの湾曲区間数e=６である。また、第n湾曲区間のアーム筐体の中心軸からワイヤまでの変位はr１=r３=１.３２＊１０-３m、r２=r４=-１.３２＊１０-３m、r５=１.４＊１０-３m、r６=-１.４＊１０-３mである。実験では、図７に示すように、連続体ロボット１００の湾曲区間の遠位端にマーカ２０１〜２０６を設け、各湾曲区間の遠位端の変位（下記の式（３０）で表す）を画像撮影装置２１０によって取得する。拡張最急降下法では、評価点数c=６０とする。そのため、湾曲区間の遠位端以外の評価点は、湾曲区間が一定曲率であると仮定し補間により求める。代表湾曲姿勢として、下記の式（３１）、式（３２）のような、全ての湾曲区間が一様に湾曲する形状と２つの変曲点を持つ形状とのk=２通りの姿勢を選ぶ。
【００４０】
また、拡張最急降下法アルゴリズムでは反復回数h=１０とし、さらに補正係数γは正数とするため、要素が負となる場合にはその値を０.１とし最適化を継続する。さらに、代表湾曲形状に加えて、運動学補正および制御性能を評価するための下記の式（３３）の形状θcitを用意する。
【００４１】
図８に、上記１.１.３）項で示した拡張最急降下法アルゴリズムによる、反復試行回数に対する補正係数γの各要素の最適化の応答を示す。図８（a）〜（e）に補正係数γp２,...,p６を、図８（f）〜（j）に補正係数γd１,...,d５を示している。補正係数γp３、γp５、γd２、γd４は、おおよそ５回の反復で収束していることが分かる。また、γp４、γd３は振動的であるが、収束の傾向を示している。残りの補正係数は振動的となっているが、本実施例の手法により補正係数の半数以上が局所的最小値を回避していることが分かる。上記式（２６）に代入し得られる補正係数ベクトルは、下記の式（３４）となる。
【００４２】
【数３０−３４】
2018008335.tif 000008
【００４３】
図９に式（３４）を用いる運動学補正による湾曲姿勢の応答を示す。図９（a）、（b）にそれぞれ式（３１）、式（３２）に示した代表形状の応答を、図９（c）に式（３３）に示した評価用形状の応答を示す。補正された運動学による応答を実線で、実験により測定される湾曲区間の遠位端の変位をアスタリスクで、比較として補正前の運動学の応答を破線で示す。図９（a）、（b）より、１.１.１）項の仮定１〜６をおいて導出した運動学では、実験による応答との誤差が生じてしまうが、その実験応答を用いて補正係数γを最適化する運動学補正により、実験応答に対する誤差を減少できることが分かる。さらに、図９（c）より、補正係数γの最適化には用いていない評価用湾曲形状においても、代表形状とほぼ同様の精度で運動学を補正することができることから、複数の代表形状を用いて複数の反復演算を行う拡張最急降下法が有効であることが分かる。
【００４４】
つぎに、１.２）節で示した補正行列Γを用いる制御系（以下、提案制御系と略す）による応答を示す。図１０、図１１に、それぞれ目標湾曲形状を式(３１)で示す第一の代表形状と、式(３３)に示す評価用形状とする応答を示す。図１０（a）、図１１（a）に提案制御系による各湾曲区間遠位端の応答を丸印で、比較として補正行列Γを用いない制御系（以下、従来制御系と略す）による応答をアスタリスクで、目標湾曲形状を破線で示す。さらに目標湾曲形状における各湾曲区間の遠位端（以下、目標遠位端座標と略す）に点を追加して示す。また、図１０（b）、図１１（b）に各湾曲区間の遠位端における目標湾曲形状と制御応答の変位の誤差について、提案制御系による応答を実線で、従来制御系による応答を破線で示す。提案制御系は、補正行列Γの逆行列を用いてワイヤ駆動変位の制御量の補正を行うことで、目標湾曲形状に対する湾曲形状の誤差を低減していることが分かる。また、代表形状と評価用形状において、誤差の低減性能に大きな差はない。このことから、複数の代表形状を用いる補正係数の最適化法が有効であることが分かる。
【００４５】
本実施例によれば、連続体ロボットの運動学に加え、隣接する湾曲区間（湾曲可能部）の連成を考慮するモデルを呈示し、前記運動学を補正する。前記モデルは、運動学を補正するための補正値（補正係数、補正係数ベクトル、補正行列）の導出に係わる。その補正値の取得では、最初に、補正しない運動学に基づいて連続体ロボットに代表形状をとらせ、そこでの湾曲区間の任意位置（例えば、遠位端）の実際の変位を測定する。そして、目標値と測定値との誤差を解消していく前記モデルを利用した最適化アルゴリズムを用いる。さらに、この運動学の補正値の逆行列などの逆値を用いて、湾曲区間の目標姿勢のデータを与えたときに取得されるアクチュエータ駆動量を補正することにより、連続体ロボットの湾曲区間の実際の姿勢の目標姿勢に対する誤差を低減する。前記補正値を用いることで、より意図通りに連続体ロボットを駆動・制御することができる。
【００４６】
（実施例２）
実施例２では、本発明を先頭追従制御へ適用する。先頭追従制御とは、図１２に示すように、最遠位端の湾曲区間が通る経路と同じ経路を後続の湾曲区間が通るように制御することである。これにより、連続体ロボットは狭小な空間をすり抜けるように進むことができる。
【００４７】
２.１）先頭追従制御の目標角度最適化
先頭追従制御は、予め経路を定められていることは必須ではなく、最遠位端の湾曲角度を後続の湾曲区間に時間差をもって連続的に伝播させれば良い。しかし、図１２に破線で示すように経路の全体が予め設定されているときは、基台部のz方向の変位に応じて湾曲形状を最適化することで制御が可能である。これにより、連続体ロボットでは、湾曲角度を伝播させる先頭追従制御よりも経路誤差を少なくすることができる。以下に手順を示す。
【００４８】
本実施例では、図１３の一点鎖線に示すように、経路がロボット全長と等しい例を考える。まず、図１３の破線に示すように、経路の近位端に接線を追加し、その終端に座標系の原点をとる。接線の長さは、ロボット全長と等しい長さとすればよい。本実施例では、この追加した接線と経路を合わせて全経路と記す。次に、全経路を等速スプライン補間などにより等間隔に２c分割し、分割された節に経路評価点P1（xtr1,ztr1）,...,P2c（xtr2c,ztr2c）を設定する。また、連続体ロボットは、実施例１と同様に、長手方向にc等分し評価点とする。先頭追従制御は、基台部の変位がzb=ztr1=0mから開始され、zb=ztrc=nlm（ここで、ｎはe以下の整数、ｌは、湾曲区間が全て等長として、その長さである）において終了する。
【００４９】
そこで、基台の前進に伴い、全経路に沿うような連続体ロボットの湾曲形状を求めるには、次のようにする。すなわち、経路評価点Pζ（１＜ζ≦c）を開始点として、全経路上からc個の経路評価点を取り出し、連続体ロボットのc個の評価点とのそれぞれの距離の総和を評価関数（下記の式（３５）で表す）とする。そして、これを最小化する湾曲角度目標値ベクトルを最急降下法により求める。基台部が第ζの経路評価点Pζにあるときの、湾曲目標角度をθrefζと書くと、先頭追従制御のためには、以上の手順でe行c列の湾曲目標角度行列Θref（下記の式（３６）で表す）を得ればよい。
【００５０】
なお、湾曲区間数が有限であるため、経路によっては式（３５）の評価関数は０にはならない。そこで、最適化により得られた湾曲角度目標値による形状と全経路の差を本実施例では機構的誤差と定義し、その湾曲角度目標値とワイヤ駆動により制御された湾曲角度の誤差を制御的誤差と定義する。
【００５１】
また、経路がロボット全長よりも短い場合は、１）近位端に追加する接線を延長する、もしくは、２）遠位端に接線を追加する、ことで前述のアルゴリズムを用いて湾曲角度目標値を生成することが可能である。１）では、経路の近位端に追加する接線の長さを、ロボットの全長+ロボットの全長-経路の長さ、と設定すればよい。また、２）では経路の遠位端に接線を追加し、その長さを、ロボットの全長-経路の長さに設定すればよい。だだし、２）では基台部の変位が経路の長さと等しくなる座標において先頭追従制御を終了する必要がある。
【００５２】
２.２）運動学補正
先頭追従制御では、取りうる経路は様々であるが、ただ一つの経路に対しても基台の進行に伴い目標湾曲形状が変動する。その中から好適な一つの湾曲形状を代表形状として選ぶのは難しく、また、全てを代表形状とすると最適化にかかる時間が膨大となる。そこで、先頭追従制御においても実施例１と同様に、a通りの代表形状を用いる拡張最急降下法により運動学補正係数γを最適化する。
【００５３】
２.３）制御系設計
先頭追従制御では、２.１節で求めた湾曲目標角度行列Θrefから、基台変位zｂに応じて、目標湾曲形状ベクトルθrefζを取り出し、実施例１と同様に補正行列Γの逆行列を用い、補正湾曲形状ベクトルを求め、ワイヤ駆動変位を求めればよい。また、先頭追従制御においても、湾曲形状の追加目標ベクトルθcomを目標湾曲形状ベクトルに加算することにより、実時間の操作による湾曲形状の制御にも適用することができる。制御系のブロック線図を図１４に示す。
【００５４】
２.４）実験
２.４.１）運動学補正評価
拡張最急降下法による運動学補正アルゴリズムの先頭追従制御に対する有効性を検証する。経路は、C字状とS字状の経路を対象とし、図１５（a）、（b）に破線で示す。湾曲区間数e=６とし、下記の式（３５）の最適化により求められる、基台変位に対する全ての湾曲形状を灰色の線で重ねて表示している。灰色の領域と破線との偏差が機構的誤差となり、経路の入り口である座標（０,nl）付近で最大の機構的誤差が現れることが分かる。拡張最急降下法に用いる代表形状は実施例１と同様に式（３１）、式（３２）の二通りを用いる。そのため、補正係数ベクトルγは実施例１と同様である。
【００５５】
図１６（a）、（b）に、それぞれ図１５（a）、（b）の軌道に対する先頭追従制御における、実験による湾曲形状と運動学による湾曲形状の誤差応答を示す。なお、本項では運動学補正の検証を行っているため、ワイヤ駆動変位は２．３節の補償を適用していない。本実施例では、基台変位zbにおける、実験による湾曲形状と運動学による形状の運動学的誤差を、下記の式（３７）のように各湾曲区間の遠位端変位の誤差の総和と定義する。
【００５６】
【数３５−３７】
2018008335.tif 000009
【００５７】
拡張最急降下法により得られる補正行列ΓCSを用いる運動学による誤差を実線で、比較として補正を行わない運動学による誤差を点線で示す。さらに本実施例では、反復による補正係数ベクトル最適化の有効性を示すため、反復を行わずに式（３１）の代表形状のみを用いて得られる補正行列ΓCによる運動学的誤差を破線で示す。また、代表形状として式（３１）、式（３２）の二通りを用いるが、反復は行わずに得られる補正行列ΓSによる運動学的誤差を一点鎖線で示す。図１６（a）、（b）の双方において、補正行列ΓCSによる応答は、補正を行わない運動学による応答と比較して、先頭追従制御の全区間において運動学的誤差を低減していることが分かる。また、ロボット全体が経路に侵入し、全ての区間が湾曲する基台変位zb=０.０４５〜０.０６mの区間では、最も運動学的誤差を低減している。これに対し、補正行列ΓCによる応答は、基台変位zb=０〜０.０２２mにおいて、補正を行わない運動学よりも誤差が大きくなってしまう。また、補正行列ΓSによる応答は、基台変位zb=０〜０.０１３mにおいて、補正を行わない運動学よりも誤差が大きくなってしまう。これより、本実施例による拡張最急降下法による補正係数最適化アルゴリズムが先端追従制御を伴う運動学補正に対して有効であることが分かる。
【００５８】
２.４.２）先端追従制御応答
次に、２.３）節で示した補正行列Γを用いる先頭追従制御系（以下、提案先頭追従制御系と略す）による応答を示す。経路は前項と同様に、図１５（a）、（b）に破線で示すC字状とS字状の経路とする。図１７の実線、破線は次の通りである。実線は、図１５（a）の経路に対して提案先頭追従制御系で算出される、基台変位zbに対応する各湾曲区間の補正目標角度θmod１,...,mod６を示す。破線は、補正を行わない制御系（以下、従来先頭追従制御系と略す）により得られる目標角度θref１,...,ref６を示す。例えば、第３湾曲区間では、zb=０.０２mで第４湾曲区間が経路侵入により湾曲を開始すると、第３湾曲区間はその湾曲に連成し正方向に湾曲するため従来先頭追従制御系では経路誤差が生ずる。この誤差を低減するため、提案先頭追従制御系はzb=０.０３m〜０.０４において第３湾曲区間の湾曲角度が負になるように目標角度を補正していることが分かる。
【００５９】
図１８、図１９にそれぞれC字状とS字状の経路に対する先頭追従制御の実験応答を示す。図１８（a）、図１９（a）は、基台変位zbに対する、各湾曲区間の遠位端座標と目標遠位端座標との誤差距離を示し、図１８（b）、図１９（b）に全ての湾曲区間の距離誤差の和を示す。また、図１８（c）、（d）それぞれ基台変位zb=０.０３m、０.０６mにおける湾曲形状を示す。提案先頭追従制御系による各湾曲区間遠位端の応答を丸印で、比較として従来先頭追従制御系による各湾曲区間遠位端の応答を×印で、基台の変位を四角印で、全経路を破線で示している。図１９（c）、（d）も同様である。
【００６０】
図１８（a）に示すC字状の経路に対する応答より、提案先頭追従制御系は、基台変位zb=０.０２m付近で第６湾曲区間が従来先頭追従制御系よりも距離誤差がやや大きくなっている。他方、図１８（b）に示す距離誤差の総和では、基台変位zb=０.０２〜０.０６mの区間では従来先頭追従制御系に対して誤差をほぼ半減している。図１８（c）より、提案先頭追従制御系による距離誤差は少ないが、従来先頭追従制御系では、第３、第４湾曲区間が経路の曲率中心側に逸脱していることが分かる。図１８（d）の経路終端到達時の形状においては、従来先頭追従制御系では、第５、第６湾曲区間が経路の曲率中心と反対側に逸脱していることが分かる。
【００６１】
図１９（a）に示すS字状の経路に対する応答より、提案先頭追従制御系は第６湾曲区間が基台変位zb=０.０４m付近で、第４湾曲区間が基台変位zb=０.０６m付近で、従来先頭追従制御系よりも距離誤差が大きい。しかし、図１９（a）に示す距離誤差の総和では、基台変位zb=０.０２〜０.０５８mの区間において従来先頭追従制御系に対して誤差を大きく低減している。図１９（c）より、提案先頭追従制御系による距離誤差は少ないが、従来先頭追従制御系では、第３〜５湾曲区間が経路の曲率中心側に逸脱していることが分かる。図１８（d）の経路終端到達時の形状においては、従来先頭追従制御系では、第６湾曲区間が経路の曲率中心と反対側に逸脱していることが分かる。このように、複数の代表形状を用いる補正行列ΓCSによる提案先頭追従制御系は、従来先頭追従制御系に比べて目標角度からの距離誤差が少なく、経路からの逸脱を低減することが可能であることが分かる。さらに、誤差低減の性能は特定の経路や、経路中の特定の区間に限らないことから、連続体ロボットの先頭追従制御に対して有効であることが分かる。
【００６２】
（実施例３）
実施例２では、経路は変曲点を持っていたが、曲率は一定としていた。本実施例では、曲率を一定としない経路に対する先頭追従制御を行う。上記実施例で用いる隣接区間連成モデルは、補正係数は湾曲角度の変動に対して一定であるが、実際は湾曲区間の連成は湾曲角度に応じて非線形に変動し、その連成の影響は湾曲角度の増加に対して飽和する傾向にある。そのため、曲率が大きい箇所がある経路では、補正が過剰となってしまうことがある。そこで、本実施例では、図２０に示すように、運動学補正に用いる行列の逆行列Γ-１に追加ゲインGmを乗ずる制御系を示す。
【００６３】
まず、本実施例で対象とする経路を図２１（a）、（b）に破線で示す。実施例２と同様に、湾曲区間数e=６とし、式（３５）を評価関数とする最適化により求まる全ての湾曲形状を灰色の線で重ねて表示している。また、本実施例では、ロボットの目標湾曲角度の最大値を６０度と制約しているため、図２１（b）に示す経路では機構的誤差は大きくなっている。本実施例では、図２１（a）の経路を用いて、追加ゲインGmを求める。手順としては、まず運動学補正係数に追加ゲインを乗じ、先頭追従制御の基台変位に対して運動学補正される湾曲形状を求め、実施例２と同様に、実験により運動学的誤差Eki（zb）を求める。次に、運動学的誤差Eki（zb）の経路全域での平均値と標準偏差を評価指標として、好適な追加ゲインGmを決定する。図２２（a）に追加ゲインGmを０から０.１おきに１まで変化させる運動学的誤差の応答を示す。運動学補正を行わない追加ゲインGm=０の応答では、経路終端の運動学的誤差が大きいことが分かる。追加ゲインGmが１に近づくにつれて、経路終端の運動学的誤差は減少するものの、基台変位zb=０.０４mm付近での運動学的誤差が増加してしまうことが分かる。追加ゲインGmを横軸として、運動学的誤差の平均値と標準偏差を縦軸に示す応答を図２２（b）に示す。これより、追加ゲインGm=０.６で運動学的誤差の平均値が最小となり、追加ゲインGm=０.９で運動学的誤差の標準偏差が最小となることが分かる。例えば、ロボットの最遠位端に撮像装置を付加すると、運動学的誤差の標準偏差を低減することで撮像された映像のぶれを少なくすることができる。これらを考慮し、本実施例では追加ゲインGmを０.７と決定する。
【００６４】
次に、追加ゲインを用いる先頭追従制御系（以下、修正先頭追従制御系と略す）による応答を示す。図２３、図２４にそれぞれ図２１（a）、（b）の経路に対する先頭追従制御の実験応答を示す。各図の（a）〜（ｄ）の軸と記号の意味は実施例２と同様である。
【００６５】
図２３（a）に示す応答より、修正先頭追従制御系は、基台変位zb=０.０２m〜０.０４mで第６湾曲区間の距離誤差が従来先頭追従制御系よりもやや大きくなっている。他方、図２３（b）に示す距離誤差の総和では、基台変位zb=０.０４５〜０.０６mの区間では従来先頭追従制御系に対して誤差を低減している。図２３（c）、（d）より、基台変位zb=０.０３mでは、修正先頭追従制御系と従来先頭追従制御系との性能に大きな差はないが、経路終端到達時の形状においては、修正先頭追従制御系は第６湾曲区間の距離誤差を低減していることが分かる。
【００６６】
図２４（a）、（b）に示す応答より、修正先頭追従制御系は全ての湾曲区間で従来先頭追従制御系よりも距離誤差を大きく低減している。図２４（c）、（d）より、基台変位zb=０.０３mでは、修正先頭追従制御系と従来先頭追従制御系との性能に大きな差はないが、経路終端到達時の形状においては、修正先頭追従制御系は第５、第６湾曲区間の距離誤差を低減していることが分かる。
【００６７】
このように、曲率を一定としない経路に対する先頭追従制御では、追加ゲインを導入することによって、従来先頭追従制御系に比べて目標角度からの距離誤差が少なく、経路からの逸脱を低減することが可能であることが分かる。さらに、誤差低減の性能は特定の経路や、経路中の特定の基台変位に限らないことから、連続体ロボットの先頭追従制御に対して有効であることが分かる。
【符号の説明】
【００６８】
１００：連続体ロボット、１０１〜１０６：湾曲区間（第１の湾曲可能部、第２の湾曲可能部）、１１１〜１１６：ワイヤ（第１のワイヤ、第２のワイヤ）、１３１〜１３６：アクチュエータ
【図１】
2018008335.tif 000010
【図２】
2018008335.tif 000011
【図３】
2018008335.tif 000012
【図４】
2018008335.tif 000013
【図５】
2018008335.tif 000014
【図６】
2018008335.tif 000015
【図７】
2018008335.tif 000016
【図８】
2018008335.tif 000017
【図９】
2018008335.tif 000018
【図１０】
2018008335.tif 000019
【図１１】
2018008335.tif 000020
【図１２】
2018008335.tif 000021
【図１３】
2018008335.tif 000022
【図１４】
2018008335.tif 000023
【図１５】
2018008335.tif 000024
【図１６】
2018008335.tif 000025
【図１７】
2018008335.tif 000026
【図１８】
2018008335.tif 000027
【図１９】
2018008335.tif 000028
【図２０】
2018008335.tif 000029
【図２１】
2018008335.tif 000030
【図２２】
2018008335.tif 000031
【図２３】
2018008335.tif 000032
【図２４】
2018008335.tif 000033
【手続補正書】
【提出日】平成28年11月13日(2016.11.13)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】００２６
【補正方法】変更
【補正の内容】
【００２６】
【数７−１２】
2018008335.tif 000034
【数１３】
2018008335.tif 000035
【数１４】
2018008335.tif 000036
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００３０
【補正方法】変更
【補正の内容】
【００３０】
【数１５−１７】
2018008335.tif 000037
【数１８】
2018008335.tif 000038
【数１９−２０】
2018008335.tif 000039
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】００４２
【補正方法】変更
【補正の内容】
【００４２】
【数３０−３２】
2018008335.tif 000040
【数３３−３４】
2018008335.tif 000041
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】００５６
【補正方法】変更
【補正の内容】
【００５６】
【数３５−３６】
2018008335.tif 000042
【数３７】
2018008335.tif 000043
【手続補正５】
【補正対象書類名】図面
【補正対象項目名】図５
【補正方法】変更
【補正の内容】
【図５】
2018008335.tif 000044
【手続補正６】
【補正対象書類名】図面
【補正対象項目名】図６
【補正方法】変更
【補正の内容】
【図６】
2018008335.tif 000045
【手続補正７】
【補正対象書類名】図面
【補正対象項目名】図８
【補正方法】変更
【補正の内容】
【図８】
2018008335.tif 000046
【手続補正８】
【補正対象書類名】図面
【補正対象項目名】図９
【補正方法】変更
【補正の内容】
【図９】
2018008335.tif 000047
【手続補正９】
【補正対象書類名】図面
【補正対象項目名】図１０
【補正方法】変更
【補正の内容】
【図１０】
2018008335.tif 000048
【手続補正１０】
【補正対象書類名】図面
【補正対象項目名】図１１
【補正方法】変更
【補正の内容】
【図１１】
2018008335.tif 000049
【手続補正１１】
【補正対象書類名】図面
【補正対象項目名】図１３
【補正方法】変更
【補正の内容】
【図１３】
2018008335.tif 000050
【手続補正１２】
【補正対象書類名】図面
【補正対象項目名】図１４
【補正方法】変更
【補正の内容】
【図１４】
2018008335.tif 000051
【手続補正１３】
【補正対象書類名】図面
【補正対象項目名】図１５
【補正方法】変更
【補正の内容】
【図１５】
2018008335.tif 000052
【手続補正１４】
【補正対象書類名】図面
【補正対象項目名】図１６
【補正方法】変更
【補正の内容】
【図１６】
2018008335.tif 000053
【手続補正１５】
【補正対象書類名】図面
【補正対象項目名】図１７
【補正方法】変更
【補正の内容】
【図１７】
2018008335.tif 000054
【手続補正１６】
【補正対象書類名】図面
【補正対象項目名】図１８
【補正方法】変更
【補正の内容】
【図１８】
2018008335.tif 000055
【手続補正１７】
【補正対象書類名】図面
【補正対象項目名】図１９
【補正方法】変更
【補正の内容】
【図１９】
2018008335.tif 000056
【手続補正１８】
【補正対象書類名】図面
【補正対象項目名】図２０
【補正方法】変更
【補正の内容】
【図２０】
2018008335.tif 000057
【手続補正１９】
【補正対象書類名】図面
【補正対象項目名】図２１
【補正方法】変更
【補正の内容】
【図２１】
2018008335.tif 000058
【手続補正２０】
【補正対象書類名】図面
【補正対象項目名】図２２
【補正方法】変更
【補正の内容】
【図２２】
2018008335.tif 000059
【手続補正２１】
【補正対象書類名】図面
【補正対象項目名】図２３
【補正方法】変更
【補正の内容】
【図２３】
2018008335.tif 000060
【手続補正２２】
【補正対象書類名】図面
【補正対象項目名】図２４
【補正方法】変更
【補正の内容】
【図２４】
2018008335.tif 000061