逐次検索法による６自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラム
【課題】逐次検索法による６自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラムを提供すること。
【解決手段】本発明での逐次検索法は任意の構造である６自由度ロボットアームに対して、図３のフローチャットの示すように手先の目標位置・姿勢に合わせるように適切な修正重みを用いて手先の位置修正と姿勢修正それぞれに対する３関節の逆運動学を逐次的に行うことにより、目標の位置と姿勢を満足する逆運動学の近似解を求める方法である。また、第６関節の角度を変数として手先の２関節の機構的な関係に基づく評価値を提案し、任意の目標位置・姿勢に対する逆運動学の解の存在と個数を確認でき、より多くの解を求められる。本発明は任意の構造を有する多関節ロボットアームの手先の目標位置・姿勢の制御および固定された手先の位置・姿勢に対するアームの多様形状の制御のために使用することができる。
【解決手段】本発明での逐次検索法は任意の構造である６自由度ロボットアームに対して、図３のフローチャットの示すように手先の目標位置・姿勢に合わせるように適切な修正重みを用いて手先の位置修正と姿勢修正それぞれに対する３関節の逆運動学を逐次的に行うことにより、目標の位置と姿勢を満足する逆運動学の近似解を求める方法である。また、第６関節の角度を変数として手先の２関節の機構的な関係に基づく評価値を提案し、任意の目標位置・姿勢に対する逆運動学の解の存在と個数を確認でき、より多くの解を求められる。本発明は任意の構造を有する多関節ロボットアームの手先の目標位置・姿勢の制御および固定された手先の位置・姿勢に対するアームの多様形状の制御のために使用することができる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御技術に関し、より詳細には、本発明の逐次検索法による６自由度ロボットアームの逆運動学を使用して多関節ロボットアームの運動制御を行う、ロボット・システム、ロボット制御方法およびプログラムに関する。
【背景技術】
【０００２】
近年、ロボット制御技術は、コンピュータの進歩に伴って進歩しており、これに対応してロボットに要求される運動はますます複雑・高度化し、またその精度についても、より高速・高精度化が必要とされている。複雑な運動制御が必要なロボットとして、例えば、６自由度ロボットアームが知られている。図１０に、これまで知られている６自由度ロボットアームのモデルを示す。
【０００３】
一般にロボットアームの制御とは手先の位置、姿勢（同様に速度や加速度の場合もある）が与えられた時に角関節の角度、角速度や角加速度を求める事である。（逆運動学の解を求める。）。
【０００４】
図１０（ａ）に示すＰＵＭＡ型ロボットアームに代表される手先３関節の軸が1点に交わる軸構成の逆運動学の解は解析的に求めることができる事が知られている。
【０００５】
しかしながら、図１０（ｂ）に示す任意の関節構成を備える６自由度ロボットアームや、図１０（ｃ）に示すＲＰＹ型の６自由度ロボットアームでは、ＰＵＭＡ型ロボットアームのような解析解は存在せず、このことが実用的なロボットアームの構造的制約を与えていた。
【０００６】
図１０（ｂ）および図１０（ｃ）に示した構造の６自由度ロボットアームに対して解析解が求まらない場合には、従来では、数値解析によって逆運動学の解を求める方法が用いられる。図１１には、逆運動学手法を使用してパラメータを決定する処理の一般例を示す。
【０００７】
図１１（ａ）は、６自由度ロボットアームのモデルを示し、図１１（ｂ）は、（ａ）に示した６自由度ロボットアームの手先配置を、位置パラメータおよび姿勢パラメータ各３つを含むパラメータにより与えるための行列式を使用する定式化を使用する。図１１（ｂ）に示すように、６自由度ロボットアームでは、逆運動学手法を用いた場合、手先配置は、各関節の回転角と手先の並進位置および配置とを変換する６×６の正方行列による行列式として定式化できる。従来では、Jacobianの逆行列Ｊ−１の近似を、Newton-Raphson法などの数値解法を使用して繰り返し計算させることにより計算し、その後、計算された結果を位置・姿勢を得るための変換行列Ｖ−１を得るために使用して、変換行列Ｖ−１の近似解を求める、複雑な処理が用いられていた。
【０００８】
ところで、図１１（ｂ）に示した運動方程式は、疎な数値要素ではなく、位置および姿勢のパラメータを含む非線形要素を与える。３×３の要素からなる３自由度までのJacobiの逆行列の計算であれば比較的容易であり、また目的とする手先配置の取得に要する時間および収束予測性（以下、計算スケーラビリティとして参照する。）も与えることができる。
【０００９】
しかしながら、非特許文献１および非特許文献２に記載するように、逆運動学的手法は、非線形要素を含む、上述した変換行列Ｖを使用しなければならないので、ロボットアームの自由度が高くなるにつれ、通常の行列式の対角化に増して計算が複雑化する。また、計算実行中に特異点が発生してエラー終了する可能性もあるなど、計算のスケーラビリティが保証できないなどの問題点があり、実用的ということができなかった。
【００１０】
上述した理由から、これまで産業用の６自由度ロボットアームの運動制御を、逆運動学を使用して制御する制御方法は、ＰＵＭＡ型のロボットアームに限定されていた。また、Newton-Raphson法などの従来の数値解析方法を、そのまま延長させただけの逆運動学手法では、任意の関節構造を有する６自由度ロボットアームを含む多自由度ロボットアームの逆運動学を解くためには、手先配置決定のスケーラビリティや、精度の問題から問題があった。
【００１１】
この他にも、逆運動学に出現する行列式を、位置行列Ｖｐと姿勢行列Ｖｏとに分解し、位置行列Ｖｐと姿勢行列Ｖｏとに対して別々に逆行列を求め、求められた位置および姿勢を使用して非線形要素の値にフィードバックさせる反復計算を行うことで、６自由度ロボットアームの逆運動学的な数値解を求める数値解法（以下、効率的解法として参照する。）が提案されている。効率的解法は、Newton-Raphson法に比較して比較的高速に一定の精度を与える数値解を与えることができる。しかしながら、効率的解法は、位置行列と姿勢行列との間の誤差および相対的重みを計算中に評価できないという問題があった。このため、初期配置および初期値設定に依存して収束せず、一気に手先の目標配置について、要求精度内で解が求められない場合があり、計算収束のスケーラビリティや精度の点で充分ではないという問題点は、依然として解決されていなかった。
【００１２】
さらに、上述した２つの解法は、任意の目標位置・姿勢に対して解の存在と個数の確認を保証できず、解の信頼性が充分ではなく、また効率的解法では８個以上の解を安定して求められない、という欠点が指摘されていた。
【００１３】
一方、６自由度ロボットアームの制御については特許文献１にも記載されている。特許文献１に記載された方法は、６自由度ロボットアームに対して指示情報を入力し姿勢が一定であるか否かの判断に応答して関節の旋回角度を計算し、関節の運動に反映させる処理を行っている。
【００１４】
この点で、逆運動学的手法を使用するということはできるものの、旋回角度を如何にして算出するか、また旋回角度の算出の収束性およびスケーラビリティに関して何ら開示するものではないし、また姿勢が一定であるか否かを判断を行う点で、ロボットアームの運動に対して適用性が限定されるという問題があった。その他、特許文献２、特許文献３、特許文献４、特許文献５、特許文献６にも多関節ロボットの制御方法が開示されているものの、多関節ロボットアームの数値解析が含む本質的問題を改善する方法については、何ら開示するものではない。
【非特許文献１】小野京右著、「メカトロニクス時代の機械力学」、培風館１９９９年、第４章（pp.89-91）
【非特許文献２】Masaharu Takano: Anew effective solution for inverse kinematics problem(synthesis) of a robotwith any type of configuration, J. of Dynamic Systems, Measurement, Control,102, pp.69-76, 1980.
【特許文献１】特開２００５−１９３３０６号公報
【特許文献２】特開２００５−２４６５４７号公報
【特許文献３】特開２００５−２３８４１１号公報
【特許文献４】特開２００５−１３１７６９号公報
【特許文献５】特開２００４−００９１７２号公報
【特許文献６】特開２００３−３４０７５４号公報
【発明の開示】
【発明が解決しようとする課題】
【００１５】
すなわち、これまで産業用の多自由度を有するロボットアームの位置・姿勢制御のために、手先の位置・姿勢に関する逆運動学の新奇なアルゴリズムを提供することが必要とされていた。
【００１６】
また、これまで、産業用の多自由度を有するロボットアームの機構設計には、ＰＵＭＡ型を選択するなどの構造の選択肢が単一であるという構造上の制約が、計算アルゴリズムのために課せられていた。このため、６自由度ロボットアームの機構に広く適用でき、逆運動学の解の収束性を高め、制御の計算スケーラビリティ、収束速度を向上させ、さらに、解析学的に等価な解よりもより多くの数値解を提供することが可能な、新奇なアルゴリズムが必要とされていた。
【課題を解決するための手段】
【００１７】
本発明者は、上述した従来技術の問題点に鑑みて検討を重ねてきたところ、任意の構造である６自由度ロボットアームの逆運動学から得られる変換行列をそのまま解くことには困難性を伴うものの、位置行列および姿勢行列といった３自由度の位置および姿勢、それぞれに関する逆運動学を解くことは、計算スケーラビリティおよび精度の点から可能であることに着目し、本発明に至ったものである。
【００１８】
すなわち、本発明は、手先の目標位置と姿勢に対応するそれぞれ３関節の位置行列Ｖｐおよび姿勢行列Ｖｏの２つの部分行列の逆行列を計算させ、この２つの逆行列の合成と６関節の逆運動学行列Ｖ−１の間に、反復計算中に誤差を評価して重み係数を計算させ、重み係数を、上記の位置および姿勢に対する２つの逆行列の反復計算にフィードバックさせて、手先の並進位置および姿勢それぞれに対する３関節の逆運動学の非線形関係を逐次的に数値計算に反映させる。この手法は本発明では逐次検索法と定義される。また、本発明では、計算された誤差の大きさに応じて決定される重み係数を導入し、多関節ロボットアームの手先の目標位置・姿勢を、計算スケーラビリティ、収束速度および精度の点で満足させつつ、逆運動学の近似解として求める。
【００１９】
すなわち、本発明によれば、６自由度の多関節ロボットアームと、前記多関節ロボットアームを制御するためのコンピュータ装置とを備えるロボット・システムであって、前記コンピュータ装置は、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数を、計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
を備える、ロボット・システムが提供できる。
【００２０】
本発明の前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行することができる。
【００２１】
本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測することができる。
【００２２】
本発明では、前記重み係数を格納する、不揮発性の記憶手段を備えることができる。
【００２３】
本発明によれば、６自由度の多関節ロボットアームを備えるロボットとコンピュータ装置とを含むロボット・システムにおけるロボット制御方法であって、前記コンピュータ装置に対して、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数計算手段により計算された前記重み係数を計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
をコンピュータ装置に実現する、方法が提供される。
【００２４】
本発明の前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行することができる。
【００２５】
本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測することができる。
【００２６】
本発明では、前記重み係数を格納する不揮発性の記憶手段から当該処理前に計算した重み係数を読み出して、前記重み係数の初期設定処理を実行することができる。
【００２７】
本発明によれば、上記いずれかに記載のロボット制御方法を実行するためのコンピュータ装置実行可能なプログラムが提供される。
【発明の効果】
【００２８】
本発明によれば、逆運動学の解の収束性および収束速度が遅いという問題点を、各反復ループに、解の収束を収束させる適切な重み係数を用いて手先の位置修正および姿勢修正を逐次的に行うことで解の発散を防止する。同時に、重み係数は、反復ループの反復回数の増加に応答して誤差を減少させるように設定され、収束速度を向上することができる。
【００２９】
さらに、本発明では、最も手先に近い関節の角度を変数とし、手先に近い２関節の位置および姿勢を使用する解評価関数を使用し、任意の目標位置・姿勢に対する逆運動学の解の存在と個数とを、高速に確認することができ、ロボットアームの手先配置の制御にフィードバックさせることができる。
【００３０】
さらに、本発明によれば、評価値を使用することにより、逐次検索法の位置修正の３関節と姿勢修正の３関節の位置制御のために使用できる、関節数の２倍以上の解を提供することを可能とし、より柔軟性のあるロボット・システム、ロボット制御方法、およびプログラムを提供することが可能となる。
【発明を実施するための最良の形態】
【００３１】
＜セクションＡ：ロボットアームの実施形態＞
図１には、本発明で使用するロボットアームの実施形態を示す。図１（ａ）が６自由度ロボットアーム１０の側面図であり、図１（ｂ）が、ロボットアームを示した関節モデル図である。図１（ａ）に示すように、ロボットアーム１０は、６自由度ロボットアームとして構成されており、固定部１２の上に第１関節１４を介して設置されている、第１関節からは、アーム２８が第２関節１６へと延びている。第２関節１６からは、アーム３０が第３関節まで延びている。第３関節１８から第４関節２０までは、アーム３２が延びている。第２関節および第３関節は、アーム２８、３０の軸に直交する方向に回転軸を有する曲げ関節リンクとされており、第１関節１４および第４関節２０は、アーム３２の軸を中心とする回転軸を有する、捻り関節リンクから形成されている。
【００３２】
第４関節からは、さらにアーム３４が第５関節２２まで延びており、第５関節からさらにアーム３６が第６関節２４まで延びている。第６関節の先端には、アーム３８を介してワーク２６が配設されていて、ロボットアームによる種々の処理を可能としている。先端部に手先として参照されるワーク２６は、本発明の手先配置を与えており、ワーク２６の並進位置および姿勢が本発明により制御される。また、図１（ｂ）に示すように、図１（ａ）に示したロボットアームは、手先の運動を、円筒座標で記述することができ、手先の位置および姿勢を、ロール(roll)角、ピッチ(pitch)角、ヨー(yaw)角の変数で記述できる。図１に示したロボットアーム１０は、ＰＵＭＡ型のロボットアームに比較して面接触作業に対して効率的に運動する機構となっている。
【００３３】
なお、図１（ｂ）では、各関節の回転軸周りの回転角を、θｉ（ｉ＝１、...、６）として示しており、それぞれの回転軸を示すカーテシアン座標系での軸を（ｘ、ｙ、ｚ）として示す。ＰＵＭＡ型の６自由度ロボットアームとは、第４関節、第５関節および第６関節が、オイラー座標系ではなく、円柱座標系で記述される点で相違する。
【００３４】
＜セクションＢ：ロボットアームのモデル・パラメータおよび数値解析的定式化＞
以下、図１に示したロボットアームの数値解析上の定式化を行う。まず、ＲＰＹ型ロボットアームの３次元空間内における位置および配置を下記式（１）で定式化する。
【００３５】
【数１】
上記式中、Ｐｘ, Ｐｙ,Ｐｚ, Ｏｘ, Ｏｙ,Ｏｚは、手先の目標配置を表し、Ｐｘ、Ｐｙ、Ｐｚは、手先の並進位置であり、Ｏｘ、Ｏｙ、Ｏｚは、手先の目標姿勢を表し、θｉは、当該関節の回転軸周りの回転角を表し、Ｖは、並進・回転を含む座標変換行列である。
【００３６】
上記式（１）に示すように、手先の目標配置が入力または設定されると、ロボットアーム１０は、関節を回動させる値を決定し、決定された値をロボットアームの駆動要素の制御データとして使用して各駆動要素へと伝送し、駆動要素が決定された値となるまでステッピング・モータや油圧モータなどを駆動させ、目標位置および目標姿勢に手先配置を移動させる。図１（ｂ）では、Ｐｉ（ｉ＝１、．．．、７）は、各関節の並進位置を示し、目標位置は、Ｐ７として規定され、Ｐ７は、第６関節のピッチ角、ロール角、ヨー角およびアーム３８の長さを指定することにより決定される。また、手先の目標姿勢は、Ｏ７で与えられる。
【００３７】
すなわち、逆運動学では、上記式（１）として定式化した関係で与えられるベクトル(Ｐｘ, Ｐｙ, Ｐｚ, Ｏｘ， Ｏｙ， Ｏｚ)を与える、ベクトル(θ1,θ2, θ3, θ4, θ5, θ6)を求める問題に帰着され、一般的には、変換行列Ｖの逆行列Ｖ−１を数値的に求める問題に帰着される。ところが、変換行列Ｖは、疎な数値要素で与えられる行列ではないので、正方行列であっても、例えば、Hauseholder対角化法などではなく、Netwon-Raphson法による数値解析的に逆行列の要素が決定される。
【００３８】
従来では、下記式（２）で与えられる６×６の変換行列を与える、ヤコビ行列Ｊに対して、そのままNewton-Raphson法を適用して逆運動学Ｊ−１の数値解が求められ、得られた解を、さらに目標配置を与えるための変換行列Ｖ−１を計算するために使用して、目標配置が決定される。
【００３９】
【数２】
【００４０】
本発明では、６自由度ロボットアームの目標位置ベクトル（Ｐｘ，Ｐｙ，Ｐｚ）と目標姿勢ベクトル（Ｏｘ，Ｏｙ， Ｏｚ）を与える３×３の部分行列の逆行列を計算する問題に置換する。３×３の部分行列の逆行列であれば、従来通り、Newton-Raphson法を使用しても充分な速度および精度で計算が収束することが保証できる。
【００４１】
図２には、本発明で使用する逆行列近似解法の基本的なアルゴリズムを示す。本発明では、図２（ａ）に示す６×６の逆行列の計算を、図２（ｂ）に示すように３×３の部分行列Ｖｐ、Ｖｏの逆行列の計算に置換して、逆行列の計算処理を軽減する。このため、本発明では従来行われていた３×３の行列を使用する逆運動学の手法およびスケーラビリティを６自由度ロボットアームの位置制御に使用することができる。なお、図２（ｂ）中、添え字ｉ、ｊ、ｋ、ｍ、ｎ、ｑは、計算に使用する関節を示す整数であり、本発明の特定の実施形態では、ｉ＝１、ｊ＝２、ｋ＝３、ｍ＝４、ｎ＝５、ｑ＝６として部分行列を計算するが、本発明では互いに連続する関節を含む３×３の要素以下の部分行列であれば、組み合わせは特に制限されるものではない。
【００４２】
＜セクションＣ：重み付け逐次反復処理による近似解＞
部分行列Ｖｐ、Ｖｏにより与えられる目標位置および目標姿勢は、位置・姿勢のクロスターム部分が欠落することになるので、クロスターム部分を排除することによる精度低下および部分行列Ｖｐ、Ｖｏの間の機構的な関係をＶｐ、Ｖｏ間の重みとして導入する。
【００４３】
本発明で使用する重み付け逐次反復処理は、手先の目標配置からの誤差の大きさが反復回数に対して確実に減少するように、ＶｐおよびＶｏに対する重み付け係数ＷｐおよびＷｏを導入する。手先の目標配置からの誤差を、それぞれ下記式（３）で与える。
【００４４】
【数３】
上記式中、ＯＰ７ｒｅｆおよびＯＯ７ｒｅｆは、それぞれ目標配置の並進位置および姿勢の値であり、ＯＰ７およびＯＯ７は、ＶｐおよびＶｏから得られた並進位置および姿勢の値である。
【００４５】
本発明では、さらに誤差の方向性をキャンセルし、収束性を高めるために、誤差の絶対の大きさを与え、さらに収束性の観点から誤差の累乗を含む、下記式（４）で与えられる重み生成関数を導入する。
【００４６】
【数４】
上記式中、Ｓは、収束性を考慮し、１以上の実数から選択することができ、本発明の特定の実施形態では、Ｓ＝２が用いられる。なお、本発明では、上述した作用を与える限り、重み生成関数の形式については特に限定されるものではない。さらに、目標配置を得るための反復計算で、重み生成関数の値が反復計算において減少するように各重みＷｐ、Ｗｏを設定する重み力学系を、下記式（５）で導入する。
【００４７】
【数５】
上記式（５）中、αｐおよびαｏは、負の定数を表し、反復計算における反復処理の変化分を規定する値に設定することができる。上記式（５）からわかるように、誤差は、反復回数Kに対して非増加とされる。このため、本発明で１以下の適当な初期値を重みを設定して反復計算を開始させることで、確実に誤差Eの極小点に停留させることが保証され、計算スケーラビリティを提供することができる。また、上記式（５）の重み力学系にしたがえば、逆に誤差を最小にする重みについても決定できることになる。また、本発明で一旦決定された重み係数を、不揮発性メモリなどに格納させておき、同様の目標位置および目標姿勢が指定された場合、反復計算の初期から不揮発性メモリに格納しておいた重みを読み込んで初期値として使用することにより、より高速な収束性を与えることができる。
【００４８】
＜セクションＤ：本発明の制御処理を使用するロボット・システムの制御方法＞
図３に本発明のロボット制御方法の処理のフローチャートを示す。図３に示す処理は、ステップＳ１００から開始し、目標位置および目標姿勢の値を取得し、ＷｐおよびＷｏの初期値を設定する。なお、ＷｐおよびＷｏの初期値は、１以下の数値を都度選択して使用することもできるし、その時点で設定された目標配置に対して以前に計算したＷｐおよびＷｏの値を割り当てておき、設定された目標位置および目標姿勢の値からＷｐおよびＷｏを検索して読み出し、重み付け係数の値として設定することができる。
【００４９】
ステップＳ１０１では、関節の回転角の初期値を設定し、ステップＳ１０２で、θ１、θ２、θ３を含む部分行列を使用して３自由度のＶｐの逆行列からＰｘ、Ｐｙ、Ｐｚ（Ｐ７）の値を計算させ、上記式（３）を使用してＶｐの逆行列から計算した値と目標位置との間の誤差を計算し、誤差の値に、上記式（５）で与えられる誤差の値から得た重み係数Ｗｐを乗じて、新たな位置ｏＰ７ｎｅｗを計算させ、同時にθ１、θ２、θ３の値を更新する。
【００５０】
ステップＳ１０３では、θ４、θ５、θ６を含む部分行列を使用して３自由度のＶｏの逆行列からＯｘ、Ｏｙ、Ｏｘ（Ｏ７）を計算させ、上記式（３）を使用してＶｏの逆行列から計算した値と目標姿勢との間の誤差を計算させ、位置計算と同様にして、新たな姿勢の値であるｏＯ７ｎｅｗを計算し、同時にθ４、θ５、θ６の値を更新する。
【００５１】
ステップＳ１０４では、誤差の値が設定したしきい値以下であるか否かを判断し、しきい値以下と判断した場合（ｙｅｓ）、ステップＳ１０５へと進み、反復計算の処理を停止させる。その後、取得した各関節のパラメータとなるようにロボットアームの駆動要素を制御して、ロボットアームの手先配置を、目標位置および目標姿勢とする。
【００５２】
また、ステップＳ１０４判断で誤差がしきい値以上であると判断した場合（ｎｏ）取得したθｉの値を使用して再度ステップＳ１０２、ステップＳ１０３、ステップＳ１０４の処理を反復させて収束するまで計算を実行させ、収束した各パラメータに対応した制御データを駆動要素に送信し、ロボットアームの制御を行う。
【００５３】
＜セクションＥ：本発明の制御方法による数値解の評価＞
本発明では、目標配置に対する誤差を最小とするために、６×６の行列Ｖを３×３の部分行列Ｖｐ、Ｖｏの逆行列の計算に置換して反復計算を実行させるが、その場合に得られる解の個数について検討した。
【００５４】
図４に、ＲＰＹ型の６自由度ロボットアーム１０を使用したロボット・システムにおける解の存在性解の個数を検討するために使用する関節モデルを示す。なお、ＰｉおよびＯｉの定義は、図１（ｂ）で説明したと同じである。６自由度ロボットアーム１０の関節モデルを、図４のように表現し、第６関節の回転角θ６を変数として、第５関節のｚ軸方向のベクトルｅ５ｚと、第４関節のｚ軸方向のベクトルｅ４ｚとを計算する。このときＰ５の値およびｅ５ｚは、Ｐ６、θ６、Ｌ６の値から直接計算する。さらに、ｅ４ｚは、Ｐ５を目標位置として第１関節、第２関節、第３関節の値を使用する３自由度の逆運動学を使用して、Ｐ４の位置および姿勢を目標として計算する。このとき、第６関節の角度θ６を変数とする、解評価関数（θ６）を下記の式で導入する。
【００５５】
【数６】
上記式中、「・」は、ベクトルの内積を表す。本発明の実施形態であるＲＰＹ型の６自由度ロボットアームでは、第４関節と第５関節とは、垂直関係が保存される。このため、解評価関数の値ｅ５ｚは、０となり、これを与えるθ６が存在するかどうかを、θ６の値を設定して、第１関節、第２関節、第３関節の逆運動学行列を数値解析することにより、目標位置および目標姿勢を与える解の存在を評価することができる。図５に横軸をθ６の値[rad]とし、縦軸に解評価関数の値をプロットした結果を示す。図５の実線および破線は、θ６の角度の位相を反対として計算した結果に対応する。図５に示されるように、本発明の６自由度のロボットアームでもＰ１〜Ｐ４の関節の運動により与えられる４つの解に対して右手系および左手系に対応する８つの解が与えられることが示されている。ただし、一般的なマニピュレータ・アームの軸構成では、０とならないことが多いが、本発明の解評価関数について、特定のロボットアームの構成について適切な評価値を使用することにより、解の存在を迅速に判断することができる。
【００５６】
さらに、本発明では、第５関節、第６関節に対しても座標系表現の異なる２つの位相の解が存在するので、解評価関数の値（評価値）がゼロになる点には、逆運動学上から考えて、２つの解が重複して存在する。このため、本発明の制御方法では、６自由度のＲＰＹ型ロボットアームの目標位置および目標姿勢に対する解を、８×２＝１６個保証することができ、従来の６自由度ロボットアームの逆運動学を使用した制御方法を用いた場合に比較して、関節数の２倍以上の多くの解を発見することが可能であることが示される。
【００５７】
＜セクションＦ：ロボット・システム＞
図６は、本発明のロボット・システムの実施形態を示した図である。本発明のロボット・システム４０は、コンピュータ装置４４により制御されており、コンピュータ装置４４は、プログラミング言語、例えば、アセンブラ、フォートラン、ＣＯＢＯＬ、Ｃ、Ｃ＋＋などによるプログラミングの下で、本発明の逆運動学手法を適用するプログラムを実装している。コンピュータ装置４４は、ロボット制御シーケンス、またはマウス５０またはキーボード４６など、からユーザ入力によって与えられた目標配置を与えるように本発明の逆運動学手法を使用して最適な数値解を決定し、ロボットアーム１０の手先配置を制御するための制御データを、バスライン５４を介してロボットアーム１０に伝送している。
【００５８】
バスライン５４は、例えばＧＰ−ＩＢなどの汎用インタフェースＩ／Ｆ５２および駆動要素への指令を行うハードウェア・インタフェースなどのソフトウェア／ハードウェアを含むインタフェース手段を介して、ロボットアーム１０へと接続されている。制御データは、ロボットアームを駆動するためのステップ・モータや油圧モータなどの駆動要素を起動して、ロボットアーム１０の手先配置を目標配置へと移動制御させる。
【００５９】
なお、本発明では、処理を実行するプログラムは、ハードディスク・ドライブ（ＨＤＤ）４８に格納しておくことができ、プログラム実行時には、不揮発性の記憶手段として、ＨＤＤ４８からプログラムを読み出してコンピュータ装置４４が処理を実行する。また、コンピュータ装置４４には、ＥＥＰＲＯＭ（図示せず）、ＥＰＲＯＭ（図示せず）またはフラッシュメモリなど、外付けまたは内蔵で不揮発性の記憶手段を、ＨＤＤ４８とは別に備えていても良く、一旦収束した重み計数ＷｏおよびＷｐを格納することができる。本発明で重み係数を不揮発性の記憶手段に格納しておく場合、コンピュータ装置４４は、同時に重み係数が計算された時のθｉの値および目標の手先配置を対応して格納させておくことができる。
【００６０】
コンピュータ装置４４は、その時点で指定された手先配置の目標値が近似であり、現在のアームの配置が当該重み係数を得た時のθｉと近似している場合には、現在のロボットアームの位置と、目標配置のデータとを使用して格納された最適な重み係数を検索し、検索された重み係数を初期設定の重み係数として設定することができる。この実施形態の場合には、過去の履歴に基づいてより最適化された重み係数から反復計算を開始させることができるので、より効率的にロボットアームの新たな位置および姿勢を決定することができる。
【００６１】
なお、本発明で使用するコンピュータ装置４４は、汎用のパーソナル・コンピュータ、またはワークステーションとして構成することもできるし、制御のみを目的としたＡＳＩＣ(Application Specific Integrated Circuit)として、ロボット・システムに実装することができる。
【００６２】
図７は、本発明のコンピュータ装置４４の機能ブロック図である。図7に示すように、コンピュータ装置４４は、本発明の処理を実行する制御処理モジュール５６を実装しており、制御処理モジュール５６は、ＩＤＥ(International Device Electronics)やＳＣＳＩ(Small Computer System Interface)などのインタフェースを介して接続されたＨＤＤ４８から、制御プログラムをＣＰＵ（図示せず）が呼び出し、実行空間を提供するＲＡＭ６０にプログラムを展開してプログラムを実行させることにより実現される。
【００６３】
コンピュータ装置４４は、ＲＯＭ５８などを備えており、ＲＯＭ５８には、ＢＩＯＳ(Basic Input Output System)などが格納されており、コンピュータ装置４４の初期設定や制御のために使用するデータをＣＰＵに渡している。なお、本発明のコンピュータ装置４４をワークステーションまたはサーバとして構成する場合には、カーネル／ＯＳ／アプリケーションの構成として記述することができる、ＵＮＩＸ（登録商標）、ＬＩＮＵＸ（登録商標）のアーキテクチャを備えるコンピュータ装置４４を使用することができる。
【００６４】
コンピュータ装置４４は、センサ／キーボード／シーケンス制御プログラムの出力などとして与えられる手先配置の目標設定値、および初期値として使用される、その時点での関節のパラメータを目標配置設定部６６が受け取り、ＲＡＭ６０に格納して以後の制御処理モジュール５６が使用可能としている。制御処理モジュール５６は、処理を開始すると、変換行列Ｖを記述した配列データから、位置行列および姿勢行列に対応した部分行列の数値要素を計算して、逆行列計算手段６８を起動し、Newton-Raphson法などを使用して逆行列を計算し、計算された手先配置の値を誤差計算手段７０に渡す。
【００６５】
誤差計算手段７０は、計算された手先配置と目標配置との間の誤差を計算し、重み係数計算手段７２に渡す。重み係数計算手段は、重み生成関数を適用して重み係数を計算し、手先配置更新手段７４に渡す。手先位置更新手段７４は、計算された誤差および重み係数を使用して手先配置を更新し、更新した値が、設定された精度内にない場合には、その結果を逆行列計算手段６８に返し、手先配置の更新計算を継続させる。一方、更新した値が設定された精度内にある場合には、その値をロボットアームの制御データとしてインタフェースＩ／Ｆ５２を介して図示しないロボットアームへと伝送する。
【００６６】
設定された精度内に収束した時に計算された重み係数は、フラッシュメモリ、ＥＥＰＲＯＭ、ＥＰＲＯＭなどで構成された記憶装置６２に格納され、後に制御処理モジュールの処理のためにデータを格納することができる。
【００６７】
なお、図７では、Ｉ／Ｆ５２をコンピュータ装置４４の外部に配置するものとして記述しているが、インタフェースＩ／Ｆ５２を、例えば、ＧＰ−ＩＢなどを使用して構成する場合には、ＧＰ−ＩＢボードは、コンピュータ装置４４に内蔵させることもできる。また、インタフェースＩ／Ｆ５２として、赤外線または近距離／遠距離無線通信インタフェースなどを使用することにより、ロボットに対してワイヤレス制御を行うことも可能である。
【００６８】
本発明の、重み係数を不揮発性の記憶手段に格納する実施形態では、従来のNewton-Raphson法では、最適化ごとに常に初期設定から数値解析を開始しなければならないが、本発明では、重み係数を最適化履歴として記憶させておき、常に現在の姿勢から最も最短で目標姿勢および目標位置に到達することが可能なロボットアームの制御を行うことができる。
【００６９】
＜セクションＧ：実施例＞
＜実施例１＞
本発明を図１に示したＲＰＹ型の６自由度ロボットアームと、図３に示した制御プログラムを実装したコンピュータとからロボット・システムを作成し、手先の制御、姿勢制御、および解の存在性および個数について、収束性および計算スケーラビリティの検討を行った。計算機実験では、コンピュータに目標配置を与えるデータを与え、計算を実行させて、反復計算の実行結果を、目標配置の誤差を反復計算の回数に対して評価することにより行った。計算は、２つの目標配置を用いて、それぞれ実施例１および実施例２とした。また、それぞれ同一のパラメータを用いてNewton-Raphson（従来例１）および効率的解法（従来例２）による計算結果をそれぞれ比較例とした。実験に使用したパラメータを下記表１に示す。また、目標位置の精度について得られた結果を図８および図９に示す。なお、本発明で使用した重み係数Ｗｐの初期値は０．７に設定した。
【００７０】
【表１】
【００７１】
図８に実施例１の結果を示す。図８に示した結果によれば、本発明の制御方法を使用した場合、Newton-Raphson法を使用した従来例１よりも、約６×１０−４ｍ以下の位置精度（６０μｍ以下）を与えるまでの反復回数は、約１７回であり、従来例１では、反復回数が約２３回でも本発明の位置精度を与えていないことが示された。なお、図８に示した反復回数を、約１００μｍの位置精度を与えるまでの計算時間で表すと、実施例１では、約１．５ｍｓであり、従来例１では、約３５ｍｓであり、約２３倍収束速度が改善された。この理由は、計算対象となる行列要素の数が減少すれば、反復計算１回あたりの計算量は、計算対象となる行列要素の数を[O]として、概ね、[Ｏ]２の２乗に相当して減少するものと考えられる。しかしながら、本発明では、これよりもはるかに収束性が改善されていることが示された。この理由は、本発明で、重み力学系を導入し、収束性およびスケーラビリティを同時に改善した結果、計算量の削減以上に反復計算の反復回数を減少させることができ、この結果、行列要素の減少の影響以上に計算時間が短縮されるためである。なお、目標姿勢についても同様の結果が得られた。
【００７２】
一方、３×３の要素を有する２つの行列を使用する従来例２では、行列ＶｐとＶｏとの間の相互的関係を考慮せずに独立して計算させるため、初期位置および初期姿勢の設定により計算が収束せず、反復計算が終了しない結果が得られた。図８に示した結果の後、反復計算を５００回まで反復計算しても、精度の改善はみられなかった。
【００７３】
以上の結果から、本発明では、ＲＰＹ型ロボットアームの制御に対して収束性および計算スケーラビリティを保証しつつ、従来にまして高速な制御を可能とすることが示された。
【００７４】
＜実施例２＞
図９には、実施例２の結果を示す。図９に示すように、実施例２では、約６０μｍの位置精度を与えるために、反復計算が約１２回（計算時間は、約１ｍｓ）であり、従来例１では、反復計算回数が約２５回（計算時間は、約４０ｍｓ）という結果が得られ、計算時間で、約４０倍の高速性が達成できることがわかった。また、効率的解法を使用した従来例２では、図９に示した結果では本発明と同程度の高速性を与えているものの、実施例１の結果を考慮すると、計算が収束するか否かについての計算スケーラビリティの信頼性に劣るということができ、また計算の高速性からみても、本発明よりも計算速度が低いことがわかる。
【００７５】
上述した実施例１および実施例２の結果により、本発明によれば、ＲＰＹ型のロボットアームの制御を、計算の収束性を保証しつつ高速化することができることが示された。
【００７６】
本発明をこれまで実施形態をもって説明してきたが、本発明は、実施形態に限定されることなく、５関節以上の関節を有し、３×３の要素の部分行列の逆運動学的解法を適用できるセグメントを有する任意の構造の６自由度の多関節ロボットアームに対して適用することができる。
【産業上の利用可能性】
【００７７】
本発明によれば、任意の関節構造を有する６自由度ロボットアームの構造に対して適用でき、従来の産業用ロボットの各関節の位置制御および姿勢制御のための制御装置を、手先配置そして、３軸目標位置および３軸回りの目標姿勢に関するより高速な制御装置に置換することができ、産業用の多関節ロボットアームの制御をより高精度・高速化することができる。
【００７８】
また、本発明によれば、従来より多くの解を、新奇で、かつ高速な手法で確認できるので、多関節ロボットアームの制御において位置・姿勢制御のための最適な解に効率的に到達させることができる。
【００７９】
このため、本発明は、溶接や研削などの産業ロボット、災害救助用ロボット、航空宇宙用途ロボットなど、ケーブル、瓦礫、その他の構造物などの障害を回避する動作が必要とされる産業用ロボット、そのための制御方法およびプログラムを提供することができ、工業上極めて有効な発明である。
【図面の簡単な説明】
【００８０】
【図１】逆運動学手法を使用してパラメータを決定する処理の従来例を示した図。
【図２】本発明で使用する逆行列近似解法の基本的なアルゴリズムを示した図。
【図３】本発明のロボット制御方法の処理のフローチャート。
【図４】ＲＰＹ型の６自由度ロボットアームを使用したロボット・システムにおける解の存在性解の個数を検討するために使用する関節モデルを示した図。
【図５】横軸をθ６の値[rad]とし、縦軸に解評価関数の値をプロットした結果を示した図。
【図６】本発明のロボット・システムの実施形態を示した図。
【図７】本発明のコンピュータ装置の機能ブロック図。
【図８】目標位置の精度について得られた結果を示した図。
【図９】目標位置の精度について得られた結果を示した図。
【図１０】従来の６自由度ロボットアームの関節モデルを示した図。
【図１１】逆運動学手法を使用してパラメータを決定する処理の従来例を示した図。
【符号の説明】
【００８１】
１０…ロボットアーム、１２…固定部、１４…第１関節、１６…第２関節、１８…第３関節、２０…第４関節、２２…第５関節、２４…第６関節、２６…ワーク、２８、３０、３２、３６、３８…アーム、４０…ロボット・システム、４２…ディスプレイ装置、４４…コンピュータ装置、４６…キーボード、４８…ＨＤＤ、５０…マウス、５２…インタフェース（Ｉ／Ｆ）、５４…バスライン、５６…制御処理モジュール、５８…ＲＯＭ、６０…ＲＡＭ、６２…記憶装置、６４…ＩＤＥ／ＳＣＳＩ、６６…目標配置設定部、６８…逆行列計算手段、７０…誤差計算手段、７２…重み係数計算手段、７４…手先配置更新手段
【技術分野】
【０００１】
本発明は、ロボット制御技術に関し、より詳細には、本発明の逐次検索法による６自由度ロボットアームの逆運動学を使用して多関節ロボットアームの運動制御を行う、ロボット・システム、ロボット制御方法およびプログラムに関する。
【背景技術】
【０００２】
近年、ロボット制御技術は、コンピュータの進歩に伴って進歩しており、これに対応してロボットに要求される運動はますます複雑・高度化し、またその精度についても、より高速・高精度化が必要とされている。複雑な運動制御が必要なロボットとして、例えば、６自由度ロボットアームが知られている。図１０に、これまで知られている６自由度ロボットアームのモデルを示す。
【０００３】
一般にロボットアームの制御とは手先の位置、姿勢（同様に速度や加速度の場合もある）が与えられた時に角関節の角度、角速度や角加速度を求める事である。（逆運動学の解を求める。）。
【０００４】
図１０（ａ）に示すＰＵＭＡ型ロボットアームに代表される手先３関節の軸が1点に交わる軸構成の逆運動学の解は解析的に求めることができる事が知られている。
【０００５】
しかしながら、図１０（ｂ）に示す任意の関節構成を備える６自由度ロボットアームや、図１０（ｃ）に示すＲＰＹ型の６自由度ロボットアームでは、ＰＵＭＡ型ロボットアームのような解析解は存在せず、このことが実用的なロボットアームの構造的制約を与えていた。
【０００６】
図１０（ｂ）および図１０（ｃ）に示した構造の６自由度ロボットアームに対して解析解が求まらない場合には、従来では、数値解析によって逆運動学の解を求める方法が用いられる。図１１には、逆運動学手法を使用してパラメータを決定する処理の一般例を示す。
【０００７】
図１１（ａ）は、６自由度ロボットアームのモデルを示し、図１１（ｂ）は、（ａ）に示した６自由度ロボットアームの手先配置を、位置パラメータおよび姿勢パラメータ各３つを含むパラメータにより与えるための行列式を使用する定式化を使用する。図１１（ｂ）に示すように、６自由度ロボットアームでは、逆運動学手法を用いた場合、手先配置は、各関節の回転角と手先の並進位置および配置とを変換する６×６の正方行列による行列式として定式化できる。従来では、Jacobianの逆行列Ｊ−１の近似を、Newton-Raphson法などの数値解法を使用して繰り返し計算させることにより計算し、その後、計算された結果を位置・姿勢を得るための変換行列Ｖ−１を得るために使用して、変換行列Ｖ−１の近似解を求める、複雑な処理が用いられていた。
【０００８】
ところで、図１１（ｂ）に示した運動方程式は、疎な数値要素ではなく、位置および姿勢のパラメータを含む非線形要素を与える。３×３の要素からなる３自由度までのJacobiの逆行列の計算であれば比較的容易であり、また目的とする手先配置の取得に要する時間および収束予測性（以下、計算スケーラビリティとして参照する。）も与えることができる。
【０００９】
しかしながら、非特許文献１および非特許文献２に記載するように、逆運動学的手法は、非線形要素を含む、上述した変換行列Ｖを使用しなければならないので、ロボットアームの自由度が高くなるにつれ、通常の行列式の対角化に増して計算が複雑化する。また、計算実行中に特異点が発生してエラー終了する可能性もあるなど、計算のスケーラビリティが保証できないなどの問題点があり、実用的ということができなかった。
【００１０】
上述した理由から、これまで産業用の６自由度ロボットアームの運動制御を、逆運動学を使用して制御する制御方法は、ＰＵＭＡ型のロボットアームに限定されていた。また、Newton-Raphson法などの従来の数値解析方法を、そのまま延長させただけの逆運動学手法では、任意の関節構造を有する６自由度ロボットアームを含む多自由度ロボットアームの逆運動学を解くためには、手先配置決定のスケーラビリティや、精度の問題から問題があった。
【００１１】
この他にも、逆運動学に出現する行列式を、位置行列Ｖｐと姿勢行列Ｖｏとに分解し、位置行列Ｖｐと姿勢行列Ｖｏとに対して別々に逆行列を求め、求められた位置および姿勢を使用して非線形要素の値にフィードバックさせる反復計算を行うことで、６自由度ロボットアームの逆運動学的な数値解を求める数値解法（以下、効率的解法として参照する。）が提案されている。効率的解法は、Newton-Raphson法に比較して比較的高速に一定の精度を与える数値解を与えることができる。しかしながら、効率的解法は、位置行列と姿勢行列との間の誤差および相対的重みを計算中に評価できないという問題があった。このため、初期配置および初期値設定に依存して収束せず、一気に手先の目標配置について、要求精度内で解が求められない場合があり、計算収束のスケーラビリティや精度の点で充分ではないという問題点は、依然として解決されていなかった。
【００１２】
さらに、上述した２つの解法は、任意の目標位置・姿勢に対して解の存在と個数の確認を保証できず、解の信頼性が充分ではなく、また効率的解法では８個以上の解を安定して求められない、という欠点が指摘されていた。
【００１３】
一方、６自由度ロボットアームの制御については特許文献１にも記載されている。特許文献１に記載された方法は、６自由度ロボットアームに対して指示情報を入力し姿勢が一定であるか否かの判断に応答して関節の旋回角度を計算し、関節の運動に反映させる処理を行っている。
【００１４】
この点で、逆運動学的手法を使用するということはできるものの、旋回角度を如何にして算出するか、また旋回角度の算出の収束性およびスケーラビリティに関して何ら開示するものではないし、また姿勢が一定であるか否かを判断を行う点で、ロボットアームの運動に対して適用性が限定されるという問題があった。その他、特許文献２、特許文献３、特許文献４、特許文献５、特許文献６にも多関節ロボットの制御方法が開示されているものの、多関節ロボットアームの数値解析が含む本質的問題を改善する方法については、何ら開示するものではない。
【非特許文献１】小野京右著、「メカトロニクス時代の機械力学」、培風館１９９９年、第４章（pp.89-91）
【非特許文献２】Masaharu Takano: Anew effective solution for inverse kinematics problem(synthesis) of a robotwith any type of configuration, J. of Dynamic Systems, Measurement, Control,102, pp.69-76, 1980.
【特許文献１】特開２００５−１９３３０６号公報
【特許文献２】特開２００５−２４６５４７号公報
【特許文献３】特開２００５−２３８４１１号公報
【特許文献４】特開２００５−１３１７６９号公報
【特許文献５】特開２００４−００９１７２号公報
【特許文献６】特開２００３−３４０７５４号公報
【発明の開示】
【発明が解決しようとする課題】
【００１５】
すなわち、これまで産業用の多自由度を有するロボットアームの位置・姿勢制御のために、手先の位置・姿勢に関する逆運動学の新奇なアルゴリズムを提供することが必要とされていた。
【００１６】
また、これまで、産業用の多自由度を有するロボットアームの機構設計には、ＰＵＭＡ型を選択するなどの構造の選択肢が単一であるという構造上の制約が、計算アルゴリズムのために課せられていた。このため、６自由度ロボットアームの機構に広く適用でき、逆運動学の解の収束性を高め、制御の計算スケーラビリティ、収束速度を向上させ、さらに、解析学的に等価な解よりもより多くの数値解を提供することが可能な、新奇なアルゴリズムが必要とされていた。
【課題を解決するための手段】
【００１７】
本発明者は、上述した従来技術の問題点に鑑みて検討を重ねてきたところ、任意の構造である６自由度ロボットアームの逆運動学から得られる変換行列をそのまま解くことには困難性を伴うものの、位置行列および姿勢行列といった３自由度の位置および姿勢、それぞれに関する逆運動学を解くことは、計算スケーラビリティおよび精度の点から可能であることに着目し、本発明に至ったものである。
【００１８】
すなわち、本発明は、手先の目標位置と姿勢に対応するそれぞれ３関節の位置行列Ｖｐおよび姿勢行列Ｖｏの２つの部分行列の逆行列を計算させ、この２つの逆行列の合成と６関節の逆運動学行列Ｖ−１の間に、反復計算中に誤差を評価して重み係数を計算させ、重み係数を、上記の位置および姿勢に対する２つの逆行列の反復計算にフィードバックさせて、手先の並進位置および姿勢それぞれに対する３関節の逆運動学の非線形関係を逐次的に数値計算に反映させる。この手法は本発明では逐次検索法と定義される。また、本発明では、計算された誤差の大きさに応じて決定される重み係数を導入し、多関節ロボットアームの手先の目標位置・姿勢を、計算スケーラビリティ、収束速度および精度の点で満足させつつ、逆運動学の近似解として求める。
【００１９】
すなわち、本発明によれば、６自由度の多関節ロボットアームと、前記多関節ロボットアームを制御するためのコンピュータ装置とを備えるロボット・システムであって、前記コンピュータ装置は、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数を、計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
を備える、ロボット・システムが提供できる。
【００２０】
本発明の前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行することができる。
【００２１】
本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測することができる。
【００２２】
本発明では、前記重み係数を格納する、不揮発性の記憶手段を備えることができる。
【００２３】
本発明によれば、６自由度の多関節ロボットアームを備えるロボットとコンピュータ装置とを含むロボット・システムにおけるロボット制御方法であって、前記コンピュータ装置に対して、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数計算手段により計算された前記重み係数を計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
をコンピュータ装置に実現する、方法が提供される。
【００２４】
本発明の前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行することができる。
【００２５】
本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測することができる。
【００２６】
本発明では、前記重み係数を格納する不揮発性の記憶手段から当該処理前に計算した重み係数を読み出して、前記重み係数の初期設定処理を実行することができる。
【００２７】
本発明によれば、上記いずれかに記載のロボット制御方法を実行するためのコンピュータ装置実行可能なプログラムが提供される。
【発明の効果】
【００２８】
本発明によれば、逆運動学の解の収束性および収束速度が遅いという問題点を、各反復ループに、解の収束を収束させる適切な重み係数を用いて手先の位置修正および姿勢修正を逐次的に行うことで解の発散を防止する。同時に、重み係数は、反復ループの反復回数の増加に応答して誤差を減少させるように設定され、収束速度を向上することができる。
【００２９】
さらに、本発明では、最も手先に近い関節の角度を変数とし、手先に近い２関節の位置および姿勢を使用する解評価関数を使用し、任意の目標位置・姿勢に対する逆運動学の解の存在と個数とを、高速に確認することができ、ロボットアームの手先配置の制御にフィードバックさせることができる。
【００３０】
さらに、本発明によれば、評価値を使用することにより、逐次検索法の位置修正の３関節と姿勢修正の３関節の位置制御のために使用できる、関節数の２倍以上の解を提供することを可能とし、より柔軟性のあるロボット・システム、ロボット制御方法、およびプログラムを提供することが可能となる。
【発明を実施するための最良の形態】
【００３１】
＜セクションＡ：ロボットアームの実施形態＞
図１には、本発明で使用するロボットアームの実施形態を示す。図１（ａ）が６自由度ロボットアーム１０の側面図であり、図１（ｂ）が、ロボットアームを示した関節モデル図である。図１（ａ）に示すように、ロボットアーム１０は、６自由度ロボットアームとして構成されており、固定部１２の上に第１関節１４を介して設置されている、第１関節からは、アーム２８が第２関節１６へと延びている。第２関節１６からは、アーム３０が第３関節まで延びている。第３関節１８から第４関節２０までは、アーム３２が延びている。第２関節および第３関節は、アーム２８、３０の軸に直交する方向に回転軸を有する曲げ関節リンクとされており、第１関節１４および第４関節２０は、アーム３２の軸を中心とする回転軸を有する、捻り関節リンクから形成されている。
【００３２】
第４関節からは、さらにアーム３４が第５関節２２まで延びており、第５関節からさらにアーム３６が第６関節２４まで延びている。第６関節の先端には、アーム３８を介してワーク２６が配設されていて、ロボットアームによる種々の処理を可能としている。先端部に手先として参照されるワーク２６は、本発明の手先配置を与えており、ワーク２６の並進位置および姿勢が本発明により制御される。また、図１（ｂ）に示すように、図１（ａ）に示したロボットアームは、手先の運動を、円筒座標で記述することができ、手先の位置および姿勢を、ロール(roll)角、ピッチ(pitch)角、ヨー(yaw)角の変数で記述できる。図１に示したロボットアーム１０は、ＰＵＭＡ型のロボットアームに比較して面接触作業に対して効率的に運動する機構となっている。
【００３３】
なお、図１（ｂ）では、各関節の回転軸周りの回転角を、θｉ（ｉ＝１、...、６）として示しており、それぞれの回転軸を示すカーテシアン座標系での軸を（ｘ、ｙ、ｚ）として示す。ＰＵＭＡ型の６自由度ロボットアームとは、第４関節、第５関節および第６関節が、オイラー座標系ではなく、円柱座標系で記述される点で相違する。
【００３４】
＜セクションＢ：ロボットアームのモデル・パラメータおよび数値解析的定式化＞
以下、図１に示したロボットアームの数値解析上の定式化を行う。まず、ＲＰＹ型ロボットアームの３次元空間内における位置および配置を下記式（１）で定式化する。
【００３５】
【数１】
上記式中、Ｐｘ, Ｐｙ,Ｐｚ, Ｏｘ, Ｏｙ,Ｏｚは、手先の目標配置を表し、Ｐｘ、Ｐｙ、Ｐｚは、手先の並進位置であり、Ｏｘ、Ｏｙ、Ｏｚは、手先の目標姿勢を表し、θｉは、当該関節の回転軸周りの回転角を表し、Ｖは、並進・回転を含む座標変換行列である。
【００３６】
上記式（１）に示すように、手先の目標配置が入力または設定されると、ロボットアーム１０は、関節を回動させる値を決定し、決定された値をロボットアームの駆動要素の制御データとして使用して各駆動要素へと伝送し、駆動要素が決定された値となるまでステッピング・モータや油圧モータなどを駆動させ、目標位置および目標姿勢に手先配置を移動させる。図１（ｂ）では、Ｐｉ（ｉ＝１、．．．、７）は、各関節の並進位置を示し、目標位置は、Ｐ７として規定され、Ｐ７は、第６関節のピッチ角、ロール角、ヨー角およびアーム３８の長さを指定することにより決定される。また、手先の目標姿勢は、Ｏ７で与えられる。
【００３７】
すなわち、逆運動学では、上記式（１）として定式化した関係で与えられるベクトル(Ｐｘ, Ｐｙ, Ｐｚ, Ｏｘ， Ｏｙ， Ｏｚ)を与える、ベクトル(θ1,θ2, θ3, θ4, θ5, θ6)を求める問題に帰着され、一般的には、変換行列Ｖの逆行列Ｖ−１を数値的に求める問題に帰着される。ところが、変換行列Ｖは、疎な数値要素で与えられる行列ではないので、正方行列であっても、例えば、Hauseholder対角化法などではなく、Netwon-Raphson法による数値解析的に逆行列の要素が決定される。
【００３８】
従来では、下記式（２）で与えられる６×６の変換行列を与える、ヤコビ行列Ｊに対して、そのままNewton-Raphson法を適用して逆運動学Ｊ−１の数値解が求められ、得られた解を、さらに目標配置を与えるための変換行列Ｖ−１を計算するために使用して、目標配置が決定される。
【００３９】
【数２】
【００４０】
本発明では、６自由度ロボットアームの目標位置ベクトル（Ｐｘ，Ｐｙ，Ｐｚ）と目標姿勢ベクトル（Ｏｘ，Ｏｙ， Ｏｚ）を与える３×３の部分行列の逆行列を計算する問題に置換する。３×３の部分行列の逆行列であれば、従来通り、Newton-Raphson法を使用しても充分な速度および精度で計算が収束することが保証できる。
【００４１】
図２には、本発明で使用する逆行列近似解法の基本的なアルゴリズムを示す。本発明では、図２（ａ）に示す６×６の逆行列の計算を、図２（ｂ）に示すように３×３の部分行列Ｖｐ、Ｖｏの逆行列の計算に置換して、逆行列の計算処理を軽減する。このため、本発明では従来行われていた３×３の行列を使用する逆運動学の手法およびスケーラビリティを６自由度ロボットアームの位置制御に使用することができる。なお、図２（ｂ）中、添え字ｉ、ｊ、ｋ、ｍ、ｎ、ｑは、計算に使用する関節を示す整数であり、本発明の特定の実施形態では、ｉ＝１、ｊ＝２、ｋ＝３、ｍ＝４、ｎ＝５、ｑ＝６として部分行列を計算するが、本発明では互いに連続する関節を含む３×３の要素以下の部分行列であれば、組み合わせは特に制限されるものではない。
【００４２】
＜セクションＣ：重み付け逐次反復処理による近似解＞
部分行列Ｖｐ、Ｖｏにより与えられる目標位置および目標姿勢は、位置・姿勢のクロスターム部分が欠落することになるので、クロスターム部分を排除することによる精度低下および部分行列Ｖｐ、Ｖｏの間の機構的な関係をＶｐ、Ｖｏ間の重みとして導入する。
【００４３】
本発明で使用する重み付け逐次反復処理は、手先の目標配置からの誤差の大きさが反復回数に対して確実に減少するように、ＶｐおよびＶｏに対する重み付け係数ＷｐおよびＷｏを導入する。手先の目標配置からの誤差を、それぞれ下記式（３）で与える。
【００４４】
【数３】
上記式中、ＯＰ７ｒｅｆおよびＯＯ７ｒｅｆは、それぞれ目標配置の並進位置および姿勢の値であり、ＯＰ７およびＯＯ７は、ＶｐおよびＶｏから得られた並進位置および姿勢の値である。
【００４５】
本発明では、さらに誤差の方向性をキャンセルし、収束性を高めるために、誤差の絶対の大きさを与え、さらに収束性の観点から誤差の累乗を含む、下記式（４）で与えられる重み生成関数を導入する。
【００４６】
【数４】
上記式中、Ｓは、収束性を考慮し、１以上の実数から選択することができ、本発明の特定の実施形態では、Ｓ＝２が用いられる。なお、本発明では、上述した作用を与える限り、重み生成関数の形式については特に限定されるものではない。さらに、目標配置を得るための反復計算で、重み生成関数の値が反復計算において減少するように各重みＷｐ、Ｗｏを設定する重み力学系を、下記式（５）で導入する。
【００４７】
【数５】
上記式（５）中、αｐおよびαｏは、負の定数を表し、反復計算における反復処理の変化分を規定する値に設定することができる。上記式（５）からわかるように、誤差は、反復回数Kに対して非増加とされる。このため、本発明で１以下の適当な初期値を重みを設定して反復計算を開始させることで、確実に誤差Eの極小点に停留させることが保証され、計算スケーラビリティを提供することができる。また、上記式（５）の重み力学系にしたがえば、逆に誤差を最小にする重みについても決定できることになる。また、本発明で一旦決定された重み係数を、不揮発性メモリなどに格納させておき、同様の目標位置および目標姿勢が指定された場合、反復計算の初期から不揮発性メモリに格納しておいた重みを読み込んで初期値として使用することにより、より高速な収束性を与えることができる。
【００４８】
＜セクションＤ：本発明の制御処理を使用するロボット・システムの制御方法＞
図３に本発明のロボット制御方法の処理のフローチャートを示す。図３に示す処理は、ステップＳ１００から開始し、目標位置および目標姿勢の値を取得し、ＷｐおよびＷｏの初期値を設定する。なお、ＷｐおよびＷｏの初期値は、１以下の数値を都度選択して使用することもできるし、その時点で設定された目標配置に対して以前に計算したＷｐおよびＷｏの値を割り当てておき、設定された目標位置および目標姿勢の値からＷｐおよびＷｏを検索して読み出し、重み付け係数の値として設定することができる。
【００４９】
ステップＳ１０１では、関節の回転角の初期値を設定し、ステップＳ１０２で、θ１、θ２、θ３を含む部分行列を使用して３自由度のＶｐの逆行列からＰｘ、Ｐｙ、Ｐｚ（Ｐ７）の値を計算させ、上記式（３）を使用してＶｐの逆行列から計算した値と目標位置との間の誤差を計算し、誤差の値に、上記式（５）で与えられる誤差の値から得た重み係数Ｗｐを乗じて、新たな位置ｏＰ７ｎｅｗを計算させ、同時にθ１、θ２、θ３の値を更新する。
【００５０】
ステップＳ１０３では、θ４、θ５、θ６を含む部分行列を使用して３自由度のＶｏの逆行列からＯｘ、Ｏｙ、Ｏｘ（Ｏ７）を計算させ、上記式（３）を使用してＶｏの逆行列から計算した値と目標姿勢との間の誤差を計算させ、位置計算と同様にして、新たな姿勢の値であるｏＯ７ｎｅｗを計算し、同時にθ４、θ５、θ６の値を更新する。
【００５１】
ステップＳ１０４では、誤差の値が設定したしきい値以下であるか否かを判断し、しきい値以下と判断した場合（ｙｅｓ）、ステップＳ１０５へと進み、反復計算の処理を停止させる。その後、取得した各関節のパラメータとなるようにロボットアームの駆動要素を制御して、ロボットアームの手先配置を、目標位置および目標姿勢とする。
【００５２】
また、ステップＳ１０４判断で誤差がしきい値以上であると判断した場合（ｎｏ）取得したθｉの値を使用して再度ステップＳ１０２、ステップＳ１０３、ステップＳ１０４の処理を反復させて収束するまで計算を実行させ、収束した各パラメータに対応した制御データを駆動要素に送信し、ロボットアームの制御を行う。
【００５３】
＜セクションＥ：本発明の制御方法による数値解の評価＞
本発明では、目標配置に対する誤差を最小とするために、６×６の行列Ｖを３×３の部分行列Ｖｐ、Ｖｏの逆行列の計算に置換して反復計算を実行させるが、その場合に得られる解の個数について検討した。
【００５４】
図４に、ＲＰＹ型の６自由度ロボットアーム１０を使用したロボット・システムにおける解の存在性解の個数を検討するために使用する関節モデルを示す。なお、ＰｉおよびＯｉの定義は、図１（ｂ）で説明したと同じである。６自由度ロボットアーム１０の関節モデルを、図４のように表現し、第６関節の回転角θ６を変数として、第５関節のｚ軸方向のベクトルｅ５ｚと、第４関節のｚ軸方向のベクトルｅ４ｚとを計算する。このときＰ５の値およびｅ５ｚは、Ｐ６、θ６、Ｌ６の値から直接計算する。さらに、ｅ４ｚは、Ｐ５を目標位置として第１関節、第２関節、第３関節の値を使用する３自由度の逆運動学を使用して、Ｐ４の位置および姿勢を目標として計算する。このとき、第６関節の角度θ６を変数とする、解評価関数（θ６）を下記の式で導入する。
【００５５】
【数６】
上記式中、「・」は、ベクトルの内積を表す。本発明の実施形態であるＲＰＹ型の６自由度ロボットアームでは、第４関節と第５関節とは、垂直関係が保存される。このため、解評価関数の値ｅ５ｚは、０となり、これを与えるθ６が存在するかどうかを、θ６の値を設定して、第１関節、第２関節、第３関節の逆運動学行列を数値解析することにより、目標位置および目標姿勢を与える解の存在を評価することができる。図５に横軸をθ６の値[rad]とし、縦軸に解評価関数の値をプロットした結果を示す。図５の実線および破線は、θ６の角度の位相を反対として計算した結果に対応する。図５に示されるように、本発明の６自由度のロボットアームでもＰ１〜Ｐ４の関節の運動により与えられる４つの解に対して右手系および左手系に対応する８つの解が与えられることが示されている。ただし、一般的なマニピュレータ・アームの軸構成では、０とならないことが多いが、本発明の解評価関数について、特定のロボットアームの構成について適切な評価値を使用することにより、解の存在を迅速に判断することができる。
【００５６】
さらに、本発明では、第５関節、第６関節に対しても座標系表現の異なる２つの位相の解が存在するので、解評価関数の値（評価値）がゼロになる点には、逆運動学上から考えて、２つの解が重複して存在する。このため、本発明の制御方法では、６自由度のＲＰＹ型ロボットアームの目標位置および目標姿勢に対する解を、８×２＝１６個保証することができ、従来の６自由度ロボットアームの逆運動学を使用した制御方法を用いた場合に比較して、関節数の２倍以上の多くの解を発見することが可能であることが示される。
【００５７】
＜セクションＦ：ロボット・システム＞
図６は、本発明のロボット・システムの実施形態を示した図である。本発明のロボット・システム４０は、コンピュータ装置４４により制御されており、コンピュータ装置４４は、プログラミング言語、例えば、アセンブラ、フォートラン、ＣＯＢＯＬ、Ｃ、Ｃ＋＋などによるプログラミングの下で、本発明の逆運動学手法を適用するプログラムを実装している。コンピュータ装置４４は、ロボット制御シーケンス、またはマウス５０またはキーボード４６など、からユーザ入力によって与えられた目標配置を与えるように本発明の逆運動学手法を使用して最適な数値解を決定し、ロボットアーム１０の手先配置を制御するための制御データを、バスライン５４を介してロボットアーム１０に伝送している。
【００５８】
バスライン５４は、例えばＧＰ−ＩＢなどの汎用インタフェースＩ／Ｆ５２および駆動要素への指令を行うハードウェア・インタフェースなどのソフトウェア／ハードウェアを含むインタフェース手段を介して、ロボットアーム１０へと接続されている。制御データは、ロボットアームを駆動するためのステップ・モータや油圧モータなどの駆動要素を起動して、ロボットアーム１０の手先配置を目標配置へと移動制御させる。
【００５９】
なお、本発明では、処理を実行するプログラムは、ハードディスク・ドライブ（ＨＤＤ）４８に格納しておくことができ、プログラム実行時には、不揮発性の記憶手段として、ＨＤＤ４８からプログラムを読み出してコンピュータ装置４４が処理を実行する。また、コンピュータ装置４４には、ＥＥＰＲＯＭ（図示せず）、ＥＰＲＯＭ（図示せず）またはフラッシュメモリなど、外付けまたは内蔵で不揮発性の記憶手段を、ＨＤＤ４８とは別に備えていても良く、一旦収束した重み計数ＷｏおよびＷｐを格納することができる。本発明で重み係数を不揮発性の記憶手段に格納しておく場合、コンピュータ装置４４は、同時に重み係数が計算された時のθｉの値および目標の手先配置を対応して格納させておくことができる。
【００６０】
コンピュータ装置４４は、その時点で指定された手先配置の目標値が近似であり、現在のアームの配置が当該重み係数を得た時のθｉと近似している場合には、現在のロボットアームの位置と、目標配置のデータとを使用して格納された最適な重み係数を検索し、検索された重み係数を初期設定の重み係数として設定することができる。この実施形態の場合には、過去の履歴に基づいてより最適化された重み係数から反復計算を開始させることができるので、より効率的にロボットアームの新たな位置および姿勢を決定することができる。
【００６１】
なお、本発明で使用するコンピュータ装置４４は、汎用のパーソナル・コンピュータ、またはワークステーションとして構成することもできるし、制御のみを目的としたＡＳＩＣ(Application Specific Integrated Circuit)として、ロボット・システムに実装することができる。
【００６２】
図７は、本発明のコンピュータ装置４４の機能ブロック図である。図7に示すように、コンピュータ装置４４は、本発明の処理を実行する制御処理モジュール５６を実装しており、制御処理モジュール５６は、ＩＤＥ(International Device Electronics)やＳＣＳＩ(Small Computer System Interface)などのインタフェースを介して接続されたＨＤＤ４８から、制御プログラムをＣＰＵ（図示せず）が呼び出し、実行空間を提供するＲＡＭ６０にプログラムを展開してプログラムを実行させることにより実現される。
【００６３】
コンピュータ装置４４は、ＲＯＭ５８などを備えており、ＲＯＭ５８には、ＢＩＯＳ(Basic Input Output System)などが格納されており、コンピュータ装置４４の初期設定や制御のために使用するデータをＣＰＵに渡している。なお、本発明のコンピュータ装置４４をワークステーションまたはサーバとして構成する場合には、カーネル／ＯＳ／アプリケーションの構成として記述することができる、ＵＮＩＸ（登録商標）、ＬＩＮＵＸ（登録商標）のアーキテクチャを備えるコンピュータ装置４４を使用することができる。
【００６４】
コンピュータ装置４４は、センサ／キーボード／シーケンス制御プログラムの出力などとして与えられる手先配置の目標設定値、および初期値として使用される、その時点での関節のパラメータを目標配置設定部６６が受け取り、ＲＡＭ６０に格納して以後の制御処理モジュール５６が使用可能としている。制御処理モジュール５６は、処理を開始すると、変換行列Ｖを記述した配列データから、位置行列および姿勢行列に対応した部分行列の数値要素を計算して、逆行列計算手段６８を起動し、Newton-Raphson法などを使用して逆行列を計算し、計算された手先配置の値を誤差計算手段７０に渡す。
【００６５】
誤差計算手段７０は、計算された手先配置と目標配置との間の誤差を計算し、重み係数計算手段７２に渡す。重み係数計算手段は、重み生成関数を適用して重み係数を計算し、手先配置更新手段７４に渡す。手先位置更新手段７４は、計算された誤差および重み係数を使用して手先配置を更新し、更新した値が、設定された精度内にない場合には、その結果を逆行列計算手段６８に返し、手先配置の更新計算を継続させる。一方、更新した値が設定された精度内にある場合には、その値をロボットアームの制御データとしてインタフェースＩ／Ｆ５２を介して図示しないロボットアームへと伝送する。
【００６６】
設定された精度内に収束した時に計算された重み係数は、フラッシュメモリ、ＥＥＰＲＯＭ、ＥＰＲＯＭなどで構成された記憶装置６２に格納され、後に制御処理モジュールの処理のためにデータを格納することができる。
【００６７】
なお、図７では、Ｉ／Ｆ５２をコンピュータ装置４４の外部に配置するものとして記述しているが、インタフェースＩ／Ｆ５２を、例えば、ＧＰ−ＩＢなどを使用して構成する場合には、ＧＰ−ＩＢボードは、コンピュータ装置４４に内蔵させることもできる。また、インタフェースＩ／Ｆ５２として、赤外線または近距離／遠距離無線通信インタフェースなどを使用することにより、ロボットに対してワイヤレス制御を行うことも可能である。
【００６８】
本発明の、重み係数を不揮発性の記憶手段に格納する実施形態では、従来のNewton-Raphson法では、最適化ごとに常に初期設定から数値解析を開始しなければならないが、本発明では、重み係数を最適化履歴として記憶させておき、常に現在の姿勢から最も最短で目標姿勢および目標位置に到達することが可能なロボットアームの制御を行うことができる。
【００６９】
＜セクションＧ：実施例＞
＜実施例１＞
本発明を図１に示したＲＰＹ型の６自由度ロボットアームと、図３に示した制御プログラムを実装したコンピュータとからロボット・システムを作成し、手先の制御、姿勢制御、および解の存在性および個数について、収束性および計算スケーラビリティの検討を行った。計算機実験では、コンピュータに目標配置を与えるデータを与え、計算を実行させて、反復計算の実行結果を、目標配置の誤差を反復計算の回数に対して評価することにより行った。計算は、２つの目標配置を用いて、それぞれ実施例１および実施例２とした。また、それぞれ同一のパラメータを用いてNewton-Raphson（従来例１）および効率的解法（従来例２）による計算結果をそれぞれ比較例とした。実験に使用したパラメータを下記表１に示す。また、目標位置の精度について得られた結果を図８および図９に示す。なお、本発明で使用した重み係数Ｗｐの初期値は０．７に設定した。
【００７０】
【表１】
【００７１】
図８に実施例１の結果を示す。図８に示した結果によれば、本発明の制御方法を使用した場合、Newton-Raphson法を使用した従来例１よりも、約６×１０−４ｍ以下の位置精度（６０μｍ以下）を与えるまでの反復回数は、約１７回であり、従来例１では、反復回数が約２３回でも本発明の位置精度を与えていないことが示された。なお、図８に示した反復回数を、約１００μｍの位置精度を与えるまでの計算時間で表すと、実施例１では、約１．５ｍｓであり、従来例１では、約３５ｍｓであり、約２３倍収束速度が改善された。この理由は、計算対象となる行列要素の数が減少すれば、反復計算１回あたりの計算量は、計算対象となる行列要素の数を[O]として、概ね、[Ｏ]２の２乗に相当して減少するものと考えられる。しかしながら、本発明では、これよりもはるかに収束性が改善されていることが示された。この理由は、本発明で、重み力学系を導入し、収束性およびスケーラビリティを同時に改善した結果、計算量の削減以上に反復計算の反復回数を減少させることができ、この結果、行列要素の減少の影響以上に計算時間が短縮されるためである。なお、目標姿勢についても同様の結果が得られた。
【００７２】
一方、３×３の要素を有する２つの行列を使用する従来例２では、行列ＶｐとＶｏとの間の相互的関係を考慮せずに独立して計算させるため、初期位置および初期姿勢の設定により計算が収束せず、反復計算が終了しない結果が得られた。図８に示した結果の後、反復計算を５００回まで反復計算しても、精度の改善はみられなかった。
【００７３】
以上の結果から、本発明では、ＲＰＹ型ロボットアームの制御に対して収束性および計算スケーラビリティを保証しつつ、従来にまして高速な制御を可能とすることが示された。
【００７４】
＜実施例２＞
図９には、実施例２の結果を示す。図９に示すように、実施例２では、約６０μｍの位置精度を与えるために、反復計算が約１２回（計算時間は、約１ｍｓ）であり、従来例１では、反復計算回数が約２５回（計算時間は、約４０ｍｓ）という結果が得られ、計算時間で、約４０倍の高速性が達成できることがわかった。また、効率的解法を使用した従来例２では、図９に示した結果では本発明と同程度の高速性を与えているものの、実施例１の結果を考慮すると、計算が収束するか否かについての計算スケーラビリティの信頼性に劣るということができ、また計算の高速性からみても、本発明よりも計算速度が低いことがわかる。
【００７５】
上述した実施例１および実施例２の結果により、本発明によれば、ＲＰＹ型のロボットアームの制御を、計算の収束性を保証しつつ高速化することができることが示された。
【００７６】
本発明をこれまで実施形態をもって説明してきたが、本発明は、実施形態に限定されることなく、５関節以上の関節を有し、３×３の要素の部分行列の逆運動学的解法を適用できるセグメントを有する任意の構造の６自由度の多関節ロボットアームに対して適用することができる。
【産業上の利用可能性】
【００７７】
本発明によれば、任意の関節構造を有する６自由度ロボットアームの構造に対して適用でき、従来の産業用ロボットの各関節の位置制御および姿勢制御のための制御装置を、手先配置そして、３軸目標位置および３軸回りの目標姿勢に関するより高速な制御装置に置換することができ、産業用の多関節ロボットアームの制御をより高精度・高速化することができる。
【００７８】
また、本発明によれば、従来より多くの解を、新奇で、かつ高速な手法で確認できるので、多関節ロボットアームの制御において位置・姿勢制御のための最適な解に効率的に到達させることができる。
【００７９】
このため、本発明は、溶接や研削などの産業ロボット、災害救助用ロボット、航空宇宙用途ロボットなど、ケーブル、瓦礫、その他の構造物などの障害を回避する動作が必要とされる産業用ロボット、そのための制御方法およびプログラムを提供することができ、工業上極めて有効な発明である。
【図面の簡単な説明】
【００８０】
【図１】逆運動学手法を使用してパラメータを決定する処理の従来例を示した図。
【図２】本発明で使用する逆行列近似解法の基本的なアルゴリズムを示した図。
【図３】本発明のロボット制御方法の処理のフローチャート。
【図４】ＲＰＹ型の６自由度ロボットアームを使用したロボット・システムにおける解の存在性解の個数を検討するために使用する関節モデルを示した図。
【図５】横軸をθ６の値[rad]とし、縦軸に解評価関数の値をプロットした結果を示した図。
【図６】本発明のロボット・システムの実施形態を示した図。
【図７】本発明のコンピュータ装置の機能ブロック図。
【図８】目標位置の精度について得られた結果を示した図。
【図９】目標位置の精度について得られた結果を示した図。
【図１０】従来の６自由度ロボットアームの関節モデルを示した図。
【図１１】逆運動学手法を使用してパラメータを決定する処理の従来例を示した図。
【符号の説明】
【００８１】
１０…ロボットアーム、１２…固定部、１４…第１関節、１６…第２関節、１８…第３関節、２０…第４関節、２２…第５関節、２４…第６関節、２６…ワーク、２８、３０、３２、３６、３８…アーム、４０…ロボット・システム、４２…ディスプレイ装置、４４…コンピュータ装置、４６…キーボード、４８…ＨＤＤ、５０…マウス、５２…インタフェース（Ｉ／Ｆ）、５４…バスライン、５６…制御処理モジュール、５８…ＲＯＭ、６０…ＲＡＭ、６２…記憶装置、６４…ＩＤＥ／ＳＣＳＩ、６６…目標配置設定部、６８…逆行列計算手段、７０…誤差計算手段、７２…重み係数計算手段、７４…手先配置更新手段
【特許請求の範囲】
【請求項１】
６自由度の多関節ロボットアームと、前記多関節ロボットアームを制御するためのコンピュータ装置を備えるロボット・システムであって、前記コンピュータ装置は、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数を、計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
を備える、ロボット・システム。
【請求項２】
前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行する。請求項１に記載のロボット・システム。
【請求項３】
前記ロボット・システムは、本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測する、請求項１または２のいずれか１項に記載のロボット・システム。
【請求項４】
前記重み係数を格納する不揮発性の記憶手段を備える、請求項１〜３のいずれか１項に記載のロボット・システム。
【請求項５】
６自由度の多関節ロボットアームを備えるロボットとコンピュータ装置を含むロボット・システムにおけるロボット制御方法であって、前記コンピュータ装置に対して、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数計算手段により計算された前記重み係数を計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
をコンピュータ装置に実現する、方法。
【請求項６】
前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行する。請求項５に記載の方法。
【請求項７】
前記ロボット・システムは、本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測する、請求項５または６のいずれかに記載の方法。
【請求項８】
前記重み係数を格納する不揮発性の記憶手段から当該処理前に計算した重み係数を読み出して、前記重み係数の初期設定処理を実行する、請求項５または６に記載の方法。
【請求項９】
請求項５〜８のいずれか１項に記載のロボット制御方法を実行するためのコンピュータ装置実行可能なプログラム。
【請求項１】
６自由度の多関節ロボットアームと、前記多関節ロボットアームを制御するためのコンピュータ装置を備えるロボット・システムであって、前記コンピュータ装置は、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数を、計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
を備える、ロボット・システム。
【請求項２】
前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行する。請求項１に記載のロボット・システム。
【請求項３】
前記ロボット・システムは、本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測する、請求項１または２のいずれか１項に記載のロボット・システム。
【請求項４】
前記重み係数を格納する不揮発性の記憶手段を備える、請求項１〜３のいずれか１項に記載のロボット・システム。
【請求項５】
６自由度の多関節ロボットアームを備えるロボットとコンピュータ装置を含むロボット・システムにおけるロボット制御方法であって、前記コンピュータ装置に対して、
前記多関節ロボットアームの手先配置を与える変換行列のうち前記手先配置の並進位置および姿勢を与える２つの部分行列の逆行列をそれぞれ計算する逆行列計算手段と、
前記部分行列の逆行列を使用して計算された前記手先配置と目標手先配置との誤差を計算させる誤差計算手段と、
前記誤差計算手段が計算した誤差の大きさに対応して計算され、前記誤差に適用する重み係数を計算する重み係数計算手段と、
前記重み係数計算手段により計算された前記重み係数を計算された前記手先配置に適用して前記手先配置を更新計算し、更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となるまで更新された前記手先配置を与える関節の回転角を修正する手先配置更新手段と、
更新された前記手先配置と前記目標手先配置との間の前記誤差の大きさが設定値以下となったことに応答して前記手先配置を前記多関節ロボットアームの制御データとして伝送する伝送手段と
をコンピュータ装置に実現する、方法。
【請求項６】
前記重み係数計算手段は、前記誤差の絶対値の大きさが前記手先配置更新手段の反復回数に対して減少する重み力学系を使用し、前記反復回数の増加に応答して前記誤差の絶対値を減少させる処理を実行する。請求項５に記載の方法。
【請求項７】
前記ロボット・システムは、本発明では、手先側に配設された関節との間の内積を評価値として、前記手先配置を与える数値解の存在を予測する、請求項５または６のいずれかに記載の方法。
【請求項８】
前記重み係数を格納する不揮発性の記憶手段から当該処理前に計算した重み係数を読み出して、前記重み係数の初期設定処理を実行する、請求項５または６に記載の方法。
【請求項９】
請求項５〜８のいずれか１項に記載のロボット制御方法を実行するためのコンピュータ装置実行可能なプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００８−３６７４２（Ｐ２００８−３６７４２Ａ）
【公開日】平成２０年２月２１日（２００８．２．２１）
【国際特許分類】
【出願番号】特願２００６−２１２１０４（Ｐ２００６−２１２１０４）
【出願日】平成１８年８月３日（２００６．８．３）
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
【公開日】平成２０年２月２１日（２００８．２．２１）
【国際特許分類】
【出願日】平成１８年８月３日（２００６．８．３）
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
[ Back to top ]