(57)【要約】【課題】力センサを用いることなくマニピュレータの先端に作用する力を高精度に求める。【解決手段】フィンガ１４の制御装置１０は、関節毎のトルクＴ１、Ｔ２、Ｔ３を求めるトルク取得部７２と、角度センサ２６ａ〜２６ｃにより関節毎の角度を検出して、フィンガ１４の姿勢に関するヤコビ行列を求める行列演算部７６と、トルク取得部７２で得られたトルクＴ１〜Ｔ３を行列演算部７６で得られたヤコビ行列に当て嵌めて、座標変換によってフィンガ１４の先端に作用する外力を演算する先端力演算部７４とを有する。トルク取得部７２は、関節毎に設けられたモータ２４ａ〜２４ｃに対するモータ指令ｕ１〜ｕ３とトルクＴ１〜Ｔ３との関係を示し、関節毎に設けられたトルクマップ８２ａ〜８２ｃを有する。トルク取得部７２は、トルクマップ８２ａ〜８２ｃを参照することによってトルクＴ１〜Ｔ３を求める。【選択図】図２
2010179442.tif 000002
【特許請求の範囲】
【請求項１】
多関節を有するマニピュレータの制御装置であって、
前記関節毎のトルクを求めるトルク取得部と、
前記関節毎の角度を検出して、前記マニピュレータの姿勢に関するヤコビ行列を求める行列演算部と、
前記トルク取得部で得られた前記トルクと前記行列演算部で得られた前記ヤコビ行列とを用いた座標変換によって前記マニピュレータの先端に発生する力を演算する先端力演算部と、
を有することを特徴とする制御装置。
【請求項２】
請求項１記載の制御装置において、
前記関節毎に設けられた駆動モータに対する駆動指令と前記トルクとの関係を示し、前記関節毎に設けられたトルクマップを有し、
前記トルク取得部は、前記トルクマップを参照することによって前記トルクを求めることを特徴とする制御装置。
【請求項３】
請求項１又は２記載の制御装置において、
前記マニピュレータの先端が発生するべき目標先端力を求める先端力指令部と、
前記目標先端力と前記先端力演算部によって求められる演算先端力との力偏差を求める力偏差演算部と、
前記力偏差を積分して前記マニピュレータの先端位置増分を求める積分部と、
センサによって検出される各軸関節角度から前記マニピュレータの先端位置を演算する現在位置演算部と、
前記先端位置増分と前記先端位置とを加算して目標となる先端位置指令を求める先端位置指令演算部と、
前記先端位置指令を行列変換して前記関節毎の指令値を求める各軸角度演算部と、
前記関節毎の指令値から前記各軸関節角度を減算して各角度偏差を求める角度偏差算出部と、
前記各角度偏差に基づいて前記マニピュレータを駆動するドライバと、
を有することとを特徴とする制御装置。
【請求項４】
請求項２又は３記載の制御装置において、
前記トルクマップは、前記駆動モータの単体の発生トルクと前記駆動指令との関係を示し、
前記トルク取得部は、前記トルクマップを参照して得られた前記駆動モータの単体の発生トルクから所定外乱力を減算して前記トルクを求めることを特徴とする制御装置。
【請求項５】
請求項２又は３記載の制御装置において、
前記トルクマップは、前記駆動モータ及び前記関節の複合的な発生トルクと前記駆動指令との関係を示すことを特徴とする制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多関節を有するマニピュレータの制御装置に関し、特に、マニピュレータの先端に作用する推定外力を演算する制御装置に関する。
【背景技術】
【０００２】
自動車の製造工程ではマニピュレータによる自動化の取り組みがなされており、例えば、予めティーチングされた動作を再現するティーチングプレイバック動作によって、溶接や塗装等の種々の処理が自動化されている。
【０００３】
一方、単なるティーチングプレイバックではなく、マニピュレータに作用する外力に応じて異なる動作が可能になると、該マニピュレータの適用範囲が一層広がることになり、特にマニピュレータの先端に作用する力が検出できると、所定のワークを適度な力で把持するような動作が可能になる。
【０００４】
マニピュレータの先端に作用する外力を検出するためには、作用点にロードセル等の力センサを設けることが考えられるが、先端部の体積増、重量増及び構造の複雑化を招き、しかも所定の向きに作用する力だけしか検出することができない。先端部の体積及び重量が大きくなると、ワークを把持する等の作業に不都合である。
【０００５】
そこで、力センサを設けることなく、ソフトウェアの処理によってマニピュレータに作用する外力を検出することが提案されている（例えば、特許文献１及び特許文献２参照）。
【０００６】
前記特許文献２記載のマニピュレータの制御装置では、関節を駆動するモータのトルク発生電圧を所定のセンサで検出し、ＡＤ変換してコンピュータに取り込み、外乱オブザーバ（９０Ａ）を介して関節トルクを演算する。その後、関節トルクをヤコビ行列に適用して先端に作用する外力を求めている（１００）。ヤコビ行列は、マニピュレータの先端の微小変位と、姿勢の微小回転角と、各関節の微小回転角との微分関係を示す行列であり、例えば、非特許文献１に説明がなされている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平９−１７９６３２号公報
【特許文献２】特開平１１−５８２８５号公報（図７）
【非特許文献】
【０００８】
【非特許文献１】広瀬茂男著、「ロボット工学  −機械システムのベクトル解析−」、第３版、株式会社裳華房、１９９０年１０月２５日、ｐ．１７２−１８３
【発明の概要】
【発明が解決しようとする課題】
【０００９】
ところが、特許文献２記載の制御装置では、規範モデルに基づく複雑な外乱オブザーバを用いているため、処理手順が煩雑であってコンピュータを用いても処理時間がかかる。また、規範モデルが適正に規定されていない場合には演算誤差が生じて十分な精度が得られない。さらに、各関節の駆動モータには、ドライバのトルク発生電圧を検出するためのセンサやその信号を読み込むＡＤ変換器が必要であって、コスト高となるとともに、検出処理に起因する制御上の遅れが発生する。
【００１０】
本発明はこのような課題を考慮してなされたものであり、力センサがなくてもマニピュレータの先端に作用する力を高精度に求めることのできる制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明に係る制御装置は、多関節を有するマニピュレータの制御装置であって、前記関節毎のトルクを求めるトルク取得部と、前記関節毎の角度を検出して、前記マニピュレータの姿勢に関するヤコビ行列を求める行列演算部と、前記トルク取得部で得られた前記トルクと前記行列演算部で得られた前記ヤコビ行列とを用いた座標変換によって前記マニピュレータの先端に発生する力を演算する先端力演算部とを有することを特徴とする。
【００１２】
このように関節毎のトルクに基づいてヤコビ行列を適用する処理によれば、力センサがなくてもマニピュレータの先端に作用する外力を高精度に求めることができる。
【００１３】
前記関節毎に設けられた駆動モータに対する駆動指令と前記トルクとの関係を示し、前記関節毎に設けられたトルクマップを有し、前記トルク取得部は、前記トルクマップを参照することによって前記トルクを求めてもよい。このようなトルクマップを設けることにより、トルクセンサがなくても駆動モータのトルクを求めることができ、しかも、複雑な演算が不要で、速い処理が実現できるとともに、演算誤差に基づく精度低下がない。
【００１４】
前記マニピュレータの先端が発生するべき目標先端力を求める先端力指令部と、前記目標先端力と前記先端力演算部によって求められる演算先端力との力偏差を求める力偏差演算部と、前記力偏差を積分して前記マニピュレータの先端位置増分を求める積分部と、センサによって検出される各軸関節角度から前記マニピュレータの先端位置を演算する現在位置演算部と、前記先端位置増分と前記先端位置とを加算して目標となる先端位置指令を求める先端位置指令演算部と、前記先端位置指令を行列変換して前記関節毎の指令値を求める各軸角度演算部と、前記関節毎の指令値から前記各軸関節角度を減算して各角度偏差を求める角度偏差算出部と、前記各角度偏差に基づいて前記マニピュレータを駆動するドライバとを有してもよい。これにより、マニピュレータの先端は所望の目標先端力に応じた力を発生することができ、例えば、ワークを把持するのに好適である。
【００１５】
前記トルクマップは、前記駆動モータの単体の発生トルクと前記駆動指令との関係を示し、前記トルク取得部は、前記トルクマップを参照して得られた前記駆動モータの単体の発生トルクから所定外乱力を減算して前記トルクを求めてもよい。これにより、外乱力（関節や減速機の摩擦等）を除去して、一層正確な先端の外力を求めることができる。また、トルクマップはマニピュレータがなくても駆動モータ単体で計測することができ、作成が容易である。
【００１６】
前記トルクマップは、前記駆動モータ及び前記関節の複合的な発生トルクと前記駆動指令との関係を示してもよい。これにより、関節（減速機等を含む）に実際に発生する外乱力が除去され、一層正確な先端の外力を求めることができる。
【発明の効果】
【００１７】
本発明に係る制御装置によれば、関節毎のトルクに基づいてヤコビ行列を適用する処理に基づき、力センサがなくてもマニピュレータの先端に作用する外力を高精度に求めることができる。
【図面の簡単な説明】
【００１８】
【図１】本実施の形態に係る制御装置により制御する把持装置の模式図である。
【図２】本実施の形態に係る制御装置のブロック構成図である。
【図３】トルクマップを示す図である。
【図４】４本のフィンガによりワークを把持する様子を示す図である。
【図５】本実施の形態に係る制御装置による実験結果を示す図である。
【発明を実施するための形態】
【００１９】
以下、本発明に係る制御装置について実施の形態を挙げ、添付の図１〜図５を参照しながら説明する。
【００２０】
図１に示すように、本実施の形態に係る制御装置１０は、把持装置１２が有する４本のフィンガ（マニピュレータ）１４の制御をする装置である。把持装置１２は、４本のフィンガ１４を用いてパレットに格納されたワーク１６を把持して搬送する。把持装置１２は、多関節ロボット１７の先端部に設けておき、ばら積み（位置、向きが不定）状態のワーク１６を多関節ロボット１７と協動して安定した状態で搬送可能となる。図１では、ワーク１６を単純な円柱形状に示すが、形状、大きさ、向きは限定されない。
【００２１】
制御装置１０は、設計条件によっては、外部のコンピュータ１１によってオフラインティーチングによって得られたデータを一部利用して把持装置１２の制御をしてもよい。
【００２２】
把持装置１２は、ベースプレート１８と、該ベースプレート１８の下面四隅にそれぞれ設けられたフィンガ１４とを有する。
【００２３】
フィンガ１４は、ベースプレート１８の下面から下方に向けて突出するように設けられており、基端部においてベースプレート１８と水平に旋回可能な第１軸１４ａと、該第１軸１４ａと直交する向きに回転可能で第１アーム２０を屈曲させる第２軸１４ｂと、該第２軸１４ｂと同じ向きに回転可能で第１アーム２０に対して第２アーム２２を屈曲させる第３軸１４ｃとを有する。フィンガ１４は、さらに、第１軸１４ａ、第２軸１４ｂ及び第３軸１４ｃ（以下、簡便に関節とも呼ぶ。）の駆動モータとしてのモータ２４ａ、モータ２４ｂ及びモータ２４ｃと、第１軸１４ａ、第２軸１４ｂ及び第３軸１４ｃの角度を検出する角度センサ２６ａ、角度センサ２６ｂ及び角度センサ２６ｃとを有する。
【００２４】
このように、フィンガ１４は３軸を有する多関節構成であり、第２アーム２２の先端部がワーク１６に当接する。４本のフィンガ１４は同構成であって、ベースプレート１８の中心部を基準として対称に設けられている。フィンガ１４には角度センサ２６ａ〜２６ｃ以外に特にセンサ（力センサやトルクセンサ等）がなく、簡便構成である。モータ２４ａ〜２４ｃ及び角度センサ２６ａ〜２６ｃは、必ずしも関節に直接的に設ける必要はなく所定の回転伝達部材を介して接続されていてもよい。モータ２４ａ〜２４ｃには減速機が設けられていてもよい。モータ２４ａ〜２４ｃにはサーボモータを用いると好適である。
【００２５】
次に、制御装置１０について説明する。制御装置１０は、フィンガ１４毎に制御部を有しており、一部又は全部をコンピュータによるソフトウェア処理によって実現している。制御装置１０は、図示しないワーク認識装置に接続されている。該ワーク認識装置は、光学手段等を用いて個別のワーク１６を識別し、その位置及び向きを示す情報を制御装置１０に供給する。制御装置１０ではワーク認識装置から得られた情報に基づいて、ワーク１６を適切に把持するべく、状況に応じて位置制御モード及び力制御モードに切換可能である。図２に制御装置１０のブロック図を示す。図２では、代表的に１本のフィンガ１４に対する制御装置１０の構成を示しているが、他の３本についても同様に構成される。図２では、力制御モードにおけるブロック図を示し、位置制御モードについては本願の発明の要旨ではないことから詳細な説明を省略する。図２は力制御モードにおけるものであるが、破線部より下側の部分は、基本構成として位置制御ループを構成している。
【００２６】
図２に示すように、制御装置１０は、各軸角度演算部６２と、各軸毎の制御補償部６４ａ、６４ｂ、６４ｃと、各軸毎の角度偏差算出部６５ａ、６５ｂ、６５ｃと、各軸毎のドライバ６６ａ、６６ｂ、６６ｃとを有する。
【００２７】
各軸角度演算部６２は、先端位置指令演算部６０によって得られる先端位置指令Ｙｃに対して、現在位置演算部６８の行列変換の逆行列により、各軸の目標角度を求める。
【００２８】
制御補償部６４ａ〜６４ｃは、角度偏差算出部６５ａ〜６５ｃで得られる偏差をＰＩＤ要素によって補償量を求めてモータ指令ｕ１、ｕ２、ｕ３とし、ドライバ６６ａ〜６６ｃ及び後述するトルク取得部７２に供給する。ドライバ６６ａ〜６６ｃは、入力信号に基づいてモータ２４ａ〜２４ｃを駆動し、これにより関節が回転することになる。
【００２９】
図２において、制御装置１０内の破線よりも上の部分は、先端位置指令Ｙｃを求める部分であり、先端位置指令演算部６０と、力偏差演算部６９と、先端力指令部７０と、トルク取得部７２と、先端力演算部７４と、行列演算部７６と、力偏差補償部７８と、積分部８０と、現在位置演算部６８とを有する。
【００３０】
現在位置演算部６８は、角度センサ２６ａ〜２６ｃの信号を読み込んで、公知の行列変換によって演算先端位置Ｙｆを求める。
【００３１】
先端位置指令演算部６０は、後述する各機能部によって求められる先端位置指令の変化量である先端位置増分ΔＹｃと現在位置演算部６８によって得られる演算先端位置Ｙｆから先端位置指令Ｙｃ＝Ｙｆ＋ΔＹｃを求める。
【００３２】
先端力指令部７０は、把持装置１２の動作状況や他の３本のフィンガ１４の状態等に基づいて、制御対象であるフィンガ１４における先端の第２アーム２２が発生すべき目標先端力Ｆｃを求める。
【００３３】
力偏差演算部６９は、先端力指令部７０から得られる目標先端力Ｆｃと先端力演算部７４から得られる演算先端力Ｆｆとの力偏差εｆを求める。目標先端力Ｆｃ及び演算先端力Ｆｆは、フィンガ１４の先端部である第２アーム２２が発生する３次元的な大きさ及び向きを示すベクトル量である。
【００３４】
力偏差補償部７８は、力偏差εｆをＰＩＤ要素によって補償量を求め、積分部８０に供給する。積分部８０では、得られた補償量を積分することにより力の量を位置の量に変換し、先端位置増分ΔＹｃを求める。先端位置指令演算部６０のＹｃ＝Ｙｆ＋ΔＹｃにより先端位置指令Ｙｃが計算され、各軸角度演算部６２に供給される。制御補償部６４ａ〜６４ｃ及び力偏差補償部７８における制御補償形式はＰＩＤに限らず、設計条件に応じてＰＩ等他の形式にしてもよい。
【００３５】
トルク取得部７２は、各軸のトルクマップ８２ａ、８２ｂ、８２ｃと、補正部８４ａ、８４ｂ、８４ｃとを有する。
【００３６】
図３に示すように、トルクマップ８２ａはモータ指令ｕ１とモータ２４ａが単体で発生するトルクＴ１0との関係を示したマップであり、所定の記憶部に記録されている。同様に、トルクマップ８２ｂ及び８２ｃは、モータ指令ｕ２、ｕ３とモータ２４ｂ、２４ｃが単体で発生するトルクＴ２0、Ｔ３0との関係を示したマップである。トルクマップ８２ａ〜８２ｃは、マップ形式に限らず実験式等であってもよい。
【００３７】
補正部８４ａは、トルクＴ１0に対してモータ２４ａによって駆動される第１軸１４ａの摩擦力等の外乱力を補正する部分であり、第１軸１４ａ及び所定の減速機等の摩擦トルクを減算し、第１軸１４ａが実質的に発生するトルクＴ１を求める。同様に、補正部８４ｂ、８４ｃは、トルクＴ２0、Ｔ３0対して第２軸１４ｂ、第３軸１４ｃ及び減速機等の摩擦トルクに基づいて補正を行い、第２軸１４ｂ及び第３軸１４ｃが実質的に発生するトルクＴ２及びＴ３を求める。
【００３８】
これらの機構部の摩擦力としては比較的大きい静摩擦と比較的小さい動摩擦があり、実際上の発生原理は複雑である。本実施の形態に係る制御装置１０では、最終的にフィンガ１４が発生する力が弱くなることがないように、余裕をもたせるため、且つ演算を簡便化するためにトルクＴ１0、Ｔ２0及びＴ３0から静摩擦力を減算することにしている。補正部８４ａ〜８４ｃによる補償値は、トルクマップ８２ａ〜８２ｃに予め含ませておいてもよい。
【００３９】
このようなトルク取得部７２では、制御補償部６４ａ〜６４ｃにより、外乱力（関節や減速機の摩擦等）を除去して、トルクＴ１〜Ｔ３が正確に求められる。また、トルクマップ８２ａ〜８２ｃはフィンガ１４がなくてもモータ２４ａ〜２４ｃ単体で計測することができ、作成が容易である。
【００４０】
トルクマップ８２ａ〜８２ｃは、モータ２４ａ〜２４ｃ及び各関節の複合的な発生トルクとモータ指令ｕ１〜ｕ３と実測データに基づいて設定してもよい。これにより、関節（減速機等を含む）に実際に発生する外乱力が除去される。
【００４１】
行列演算部７６は、角度センサ２６ａ〜２６ｃから供給される角度信号に基づいて、（１）式で示すフィンガ１４のヤコビ行列Ｊを求める。
【００４２】
【数１】
2010179442.tif 000003
【００４３】
ここで、ｑは、角度センサ２６ａ〜２６ｃによって得られる第１軸１４ａ、第２軸１４ｂ及び第３軸１４ｃの関節角度であり、ｐは第２アーム２２の先端位置である。Ｔは転置行列を示す。この（１）式に基づいて、次の（２）式が導出される。
【００４４】
【数２】
2010179442.tif 000004
【００４５】
ここで、τは、関節に発生するトルク、すなわちトルクＴ１、Ｔ２、Ｔ３であり、演算先端力Ｆｆは第２アーム２２の先端部が発生する力であり、３次元的なベクトル量である。ＪTを詳細に示すと、次の（３）式のようになる。
【００４６】
【数３】
2010179442.tif 000005
【００４７】
ここで、0Ｓ1、0Ｓ2、0Ｓ3は第１軸１４ａ、第２軸１４ｂ及び第３軸１４ｃの回転方向ベクトルであり、0Ｐ1、0Ｐ2、0Ｐ3は第１軸１４ａ、第２軸１４ｂ及び第３軸１４ｃの位置ベクトルであり、×はベクトル積を示す演算子であり、ｒは第２アーム２２の先端を示す識別子である。この（２）式及び（３）式に基づいて、次の（４）式が導出される。
【００４８】
【数４】
2010179442.tif 000006
【００４９】
ここで、Ｊ（ｑ）T+は、はＪ（ｑ）Tの擬似逆行列である。
【００５０】
この（４）式によれば、トルクＴ１、Ｔ２、Ｔ３によって先端部が発生する力としての演算先端力Ｆｆを演算可能であることが理解されよう。
【００５１】
行列演算部７６では、角度センサ２６ａ〜２６ｃの信号に基づいて、（４）式に相当する行列式を求める。
【００５２】
次いで、先端力演算部７４では、行列演算部７６によって得られた行列式に、トルクＴ１、Ｔ２及びＴ３を適用することによって演算先端力Ｆｆを求め、力偏差演算部６９に供給する。次いで、先端力演算部７４と行列演算部７６は、理解が容易となるように区別しているが、実質的には１つの処理部で行ってもよい。
【００５３】
次に、このように構成される制御装置１０の作用について説明する。
【００５４】
先ず、所定の位置制御モードによって、４つの各フィンガ１４の目標姿勢を決定して、該姿勢となるように制御をしておき、ワーク１６を把持する。
【００５５】
次いで、１以上のフィンガ１４について、所定の把持条件等に基づき、先端力指令部７０によってベクトル量としての目標先端力Ｆｃを求める。
【００５６】
フィンガ１４は４本あることから、例えば、図４に示すように、隣り合う第１のフィンガ１５ａ及び第４のフィンガ１５ｄについては、位置制御モードのままにしてその位置を固定しておき、第２のフィンガ１５ｂ及び第３のフィンガ１５ｃについて力制御モードとして目標先端力Ｆｃを設定して制御してもよい。これにより、位置の第１のフィンガ１５ａ及び第４のフィンガ１５ｄは、第１のフィンガ１５ａ及び第４のフィンガ１５ｄの発生する力に応じて反力Ｆｘを発生し、ワーク１６を把持することができる。
【００５７】
一方、トルク取得部７２では、その時点の関節に発生するトルクＴ１、Ｔ２、Ｔ３をモータ指令ｕ１、ｕ２、ｕ３からトルクマップ８２ａ、８２ｂ、８２ｃ及び補正部８４ａ、８４ｂ、８４ｃにより求める。また、行列演算部７６では、角度センサ２６ａ〜２６ｃから得られる角度信号に基づいて、前記（４）式に相当する式を求める。
【００５８】
先端力演算部７４では、行列演算部７６によって得られる式及びトルク取得部７２から供給されるトルクＴ１〜Ｔ３に基づいて、その時点の演算先端力Ｆｆを求める。
【００５９】
力偏差演算部６９では力偏差εｆを求め、力偏差補償部７８で所定の補償処理を行った後、積分部８０によって力の量を位置の量に変換し、先端位置増分ΔＹｃを求める。先端位置指令演算部６０では、先端位置増分ΔＹｃと演算先端位置Ｙｆを加算して先端位置指令Ｙｃを求め、該先端位置指令Ｙｃから各軸角度演算部６２により関節の角度を求め、さらに、各軸毎に角度偏差算出部６５ａ〜６５ｃによって角度偏差を求めて制御補償部６４ａ〜６４ｃによりモータ指令ｕ１、ｕ２、ｕ３が求められる。
【００６０】
このような制御装置１０によれば、力偏差演算部６９を減算点とする力のフィードバックループが形成されていることから、演算先端力Ｆｆは次第に目標先端力Ｆｃに近づき、力偏差εｆは収束することになる。
【００６１】
また、演算先端力Ｆｆは、角度センサ２６ａ〜２６ｃ及びモータ指令ｕ１〜ｕ３に基づいて求められるのであって、電流センサ、トルクセンサ、力センサ等が不要であり、これらの入力インターフェースも不要である。特に、先端の第２アーム２２にセンサが必要ないことから、該第２アーム２２を軽量且つ細く形成することができ、狭所に挿入可能になるとともに重量負荷が小さいために制御性に優れる。
【００６２】
フィンガ１４には角度センサ２６ａ〜２６ｃが設けられているが、これは一般的なマニピュレータには当然に設けられているものであり、これにより構成が特に複雑になることはない。
【００６３】
さらに、演算先端力Ｆｆを求める際には、複雑な規範モデルに基づく演算が不要であって処理時間が短く、該規範モデルによる演算誤差がない。
【００６４】
さらにまた、制御装置１０では、演算先端力Ｆｆを求める基準としてのトルクＴ１、Ｔ２、Ｔ３は、トルク取得部７２におけるトルクマップ８２ａ〜８２ｃを参照することによって、簡便且つ迅速に求めることができる。トルクマップ８２ａ〜８２ｃは、実際のモータ２４ａ〜２４ｃの出力に基づいて設定されており、しかも補正部８４ａ〜８４ｃによって摩擦力等の外力を補償することから、トルクＴ１〜Ｔ３は、実際に関節が発生するトルクに相当近くなり、最終的に得られる演算先端力Ｆｆも高精度になる。
【００６５】
本発明者は、制御装置１０の制御過程で得られる演算先端力Ｆｆの精度について確認する実験を行い、図５に示す結果を得た。この実験では、目標先端力Ｆｃを０から２０Ｎにステップ的に変化させ、実際の先端力Ｆｒと演算先端力Ｆｆの応答を記録した。先端力Ｆｒは、第２アーム２２の先端に力センサを設けて計測した。
【００６６】
この結果、先端力Ｆｒは最終的に２７Ｎ程度になり、専用のセンサがなくても、ワーク１６を把持するのに実用上問題のない程度の精度が得られることが確認された。全体的に先端力Ｆｒよりも計算による演算先端力Ｆｆの方が小さく求められているのは、トルク取得部７２の補正部８４ａ〜８４ｃにおいて静摩擦力に相当する外力を減算していることに起因すると考えられ、この補正部８４ａ〜８４ｃにおいて、状況に応じて動摩擦を減算するようにすると一層高精度に演算先端力Ｆｆが求められる。
【００６７】
また、計算によって求められる演算先端力Ｆｆは、目標先端力Ｆｃと同じ２０Ｎに収束している。さらに、その収束速度は、０．４ｓｅｃ程度で十分に高速であり、無駄なオーバシュートもほとんどない。図示を省略するが、目標先端力Ｆｃを５Ｎ、１０Ｎ及び１５Ｎとした場合にも同様の好適な結果が得られた。
【００６８】
制御装置１０によって制御される把持装置１２のフィンガ１４の数は４本に限らず、例えば、２本や３本でもよい。また、可動のフィンガ１４は１本として、ワーク１６を挟持する相手方部材は固定部材としてもよい。
【００６９】
フィンガ１４の関節の数は３に限らず、２又は４以上であってもよい。
【００７０】
本発明に係る制御装置は、上述の実施の形態に限らず、本発明の要旨を逸脱することなく、種々の構成を採り得ることはもちろんである。
【符号の説明】
【００７１】
１０…制御装置                              １２…把持装置
１４…フィンガ                              １４ａ…第１軸
１４ｂ…第２軸                              １４ｃ…第３軸
１６…ワーク                                １８…ベースプレート
２０…第１アーム                            ２２…第２アーム
２４ａ〜２４ｃ…モータ                      ２６ａ〜２６ｃ…角度センサ
６０…先端位置指令演算部                    ６２…軸角度演算部
６４ａ〜６４ｃ…制御補償部                  ６６ａ〜６６ｃ…ドライバ
６８…現在位置演算部                        ６９…力偏差演算部
７０…先端力指令部                          ７２…トルク取得部
７４…先端力演算部                          ７６…行列演算部
７８…力偏差補償部                          ８０…積分部
８２ａ〜８２ｃ…トルクマップ                ８４ａ〜８４ｃ…補正部
Ｆｃ…目標先端力                            Ｆｆ…演算先端力
Ｆｒ…先端力                                Ｔ１〜Ｔ３…トルク
Ｙｃ…先端位置指令                          Ｙｆ…演算先端位置
ｕ１〜ｕ３…モータ指令
【図１】
2010179442.tif 000007
【図２】
2010179442.tif 000008
【図３】
2010179442.tif 000009
【図４】
2010179442.tif 000010
【図５】
2010179442.tif 000011