JP2019093486A - ロボットの制御装置及びロボットの逆変換処理方法 - Google Patents

Abstract

【課題】オフセットアームを備えるロボットについても、逆変換処理を行うことができるロボットの制御装置を提供する。【解決手段】コントローラ３は、５軸が配置され４軸と６軸とを連結するリンク長ｄ５の第３オフセットアーム８を有し、４軸の軸心と６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボット２を制御対象とする。アームの先端部である手先を制御点とし、制御点の目標となる位置及び姿勢を逆変換処理して各軸の角度を算出する。第６軸の角度を仮決めしてθ6Aすると、その角度からオフセットアーム８の方向を求め、第６軸の目標位置Ｐ6からリンク長ｄ５を減じた仮目標位置Ｐ6Aを求めると、仮目標位置ＰAに対しリンク長ｄ５を「０」に設定して逆変換処理を行う。逆変換処理の結果を評価関数により評価することで、求められた６軸の角度θ6と仮決めした第６軸の角度θ6Aとの差が所定値以下となるまで、６軸角度の仮決めから処理を繰り返し実行する。【選択図】図７

Description

本発明は、第４軸と第６軸とを連結するオフセットアームを有するロボットの動作を制御する装置,及び手先位置を逆変換処理する方法に関する。

一般的な垂直６軸型のロボットアームは、第４軸〜第６軸の各軸心が一点で交差する構造となっている。このようなロボットアームでは、第５軸を回転させて第６軸を含む手首部分を変位させるため、アームの先端部において手首を保持している部分に隙間が形成されている。そのような隙間があると、異物を挟み込んでしまう可能性が有る。そこで、第４軸と第６軸とを連結するオフセットアームを備えることで、手首が変位する部分の隙間をなくした形態の産業用ロボットが、例えば意匠登録１５８３７５５等に開示されている。この形態では、オフセットアームの存在により、第４軸の軸心と第６軸の軸心とが交差することなく互いに平行な関係となっている。

このような形態のアームについて、手先の位置・姿勢から各軸の角度を求める逆変換処理を行うことを想定すると、従来の手法では困難である。例えば特許文献１に開示されているのは、ロボット組み付け時の誤差を含む場合でもＤＨパラメータに基づく逆変換処理を行うことを可能にした技術である。特許文献１では、オフセットアームの長さをｄ５とすると、ｄ５＝０であることを前提に、前記誤差に対応する微小領域を仮定して線形化することで収束演算を行っている。

特開２０１７−６１０２２号公報

ｄ５≠０のロボットに特許文献１の手法を適用することを想定すると、収束演算の初期誤差としてのｄ５が大きな値となるため、演算の繰り返し回数が膨大になったり、演算が収束せずに発散する可能性が有る。また、オフセット分があることで、本来はリーチが届く範囲であってもオフセット分が正しく反映されず、リーチが届かないと誤判定される可能性がある。

本発明は、上記実情に鑑みてなされたものであり、その目的は、オフセットアームを備えるロボットについても、逆変換処理を行うことができるロボットの制御装置,及びロボットの逆変換処理方法を提供することにある。

請求項１記載のロボットの制御装置は、第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットを制御対象とする。そして、アームの先端部を制御点とし、その制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出する。

角度仮決め部が第６軸の角度を仮決めすると、方向算出部は、前記角度からオフセットアームの方向を求め、仮目標位置算出部は、第６軸の目標位置から、リンク長ｄ５を減じた仮目標位置を求める。逆変換処理部は、仮目標位置に対し、リンク長ｄ５を「０」に設定して逆変換処理を行う。そして、評価部は、逆変換処理の結果を評価関数により評価することで、求められた第６軸の角度と、仮決めした第６軸の角度との差が所定値以下となるまで、角度仮決め部による仮決めから処理を繰り返し実行する。
このように構成すれば、第６軸の角度を仮決めすることで、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有するロボットについても、逆変換処理を行うことが可能になる。

請求項２記載のロボットの制御装置によれば、第３軸，第４軸のアプローチベクトルをそれぞれａ３，ａ４とすると、評価部は、内積ａ３・ａ４を評価関数して用いる。すなわち、逆変換処理が適切に行われていれば、アプローチベクトルａ３とａ４とが直交しておりそれらの内積がゼロになるはずである。したがって、逆変換処理の結果を適切に評価できる。

請求項３記載のロボットの制御装置によれば、形態決定部は、同一の手先位置について第４軸から第６軸の何れかの複数の角度を予め採番しておくことで、逆変換処理結果の手首形態を決定する。すなわち、リンク長ｄ５のオフセットアームを有するロボットは、同一の手先位置について最大で４つの手首形態を採り得る。そこで、複数の手首形態のうち、採用する手首形態に対応する第４軸から第６軸の何れかの角度を、小ささ順又は大きさ順に予め採番しておく。そして、逆変換結果として得られた対応する軸の角度が、予め採番した大きさに対応するものに一致すれば、その逆変換結果を採用する。これにより、手首形態を一意に決定できる。

請求項４記載のロボットの制御装置によれば、形態決定部は、逆変換処理の結果についてヤコビ行列の行列式を演算する。そして、行列式の符号が予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する。例えば、各軸の可動範囲が−１８０度〜１８０度である場合に、５軸を動かして手首形態を目視で確認すると、最大で２つの腕形態,２つの肘形態及び４つの手首形態の組み合わせとなることが分かる。

そして、４つの手首形態の境界を分ける点は特異点となるはずであり、ヤコビ行列は、特異点において行列式がゼロとなる。そこで、各形態の組み合わせについて、逆変換処理結果の手首形態とヤコビ行列の行列式の符号とが対応するか否かにより、その形態が指定したものとなっているか否かを判定できる。すなわち、４軸の角度が−９０度〜９０度，−１８０度〜−９０度又は９０度〜１８０度の何れに属するかと組み合わせることで、最大で４つの手首形態を判別できる。

第１実施形態であり、ロボットシステムの構成を示す図 ロボットの座標系を示す図 ロボットの構成を、ｘｚ平面及びｙｚ平面で示す図 ロボットのアーム形態をｘｙ座標及びαｒ座標で示す図 ロボットのアーム形態をαｚ座標で示す図 αｚ座標で定義したアームの各形態を示す図 コントローラが行う逆変換処理を示すフローチャート ｄ２＝ｄ３である場合の動作範囲を説明する図 同一の手先位置についてロボットアームが取り得る４つの手首形態を説明する図 第２実施形態であり、コントローラが行う逆変換処理を示すフローチャート 第３実施形態であり、コントローラが行う逆変換処理を示すフローチャート

（第１実施形態）
以下、第１実施形態について図１から図９を参照して説明する。図１に示すように、ロボットシステム１は、垂直多関節型のロボット２、ロボット２を制御するコントローラ３をベース４の内部に備えている。このロボットシステム１は、一般的な産業用に用いられている。ロボット２は、いわゆる６軸の垂直多関節型ロボットである。ベース４上に、ｚ方向の軸心を持つ第１軸；Ｊ１を介してショルダ５が水平方向に回転可能に連結されている。ショルダ５には、ｙ方向の軸心を持つ第２軸；Ｊ２を備え、ｙ方向に伸びる第２オフセットアーム６を介して、上方に延びる第１アーム７の下端部が垂直方向に回転可能に連結されている。第１アーム７の先端部には、Ｙ方向の軸心を持つ第３軸；Ｊ３を備え、−ｙ方向に伸びる第３オフセットアーム８を介して、第２アーム９が垂直方向に回転可能に連結されている。第２アーム９は、基部９ａ及び先端部９ｂからなる。

第２アーム９は、ｘ方向の軸心を持つ第４軸；Ｊ４を備え、基部９ａに対して先端部９ｂが捻り回転可能に連結されている。第２アーム９の先端部には、ｙ方向の軸心を持つ第５軸；Ｊ５を備え、−ｙ方向に伸びる第５オフセットアーム１０を介して、手首１１が垂直方向に回転可能に連結されている。手首１１には、ｘ方向の軸心を持つ第６軸；Ｊ６を介して、図２に示すフランジ及びハンド１２が捻り回転可能に連結されている。ロボット２に設けられている各軸；Ｊ１〜Ｊ６には、それぞれに対応して駆動源となる図示しないモータが設けられている。

コントローラ３は、ロボット２の制御装置であり、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御手段においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりそれぞれのモータを駆動する。

コントローラ３は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット２のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。コントローラ３は、角度仮決め部,方向算出部,仮目標位置算出部,逆変換処理部,評価部及び形態決定部に相当する。位置検出回路には、ロボット２の各関節に設けられた図示しない各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。

コントローラ３は、予め設定された動作プログラムを実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置及び姿勢を制御する。本実施形態において、コントローラ３は、ＣＰ（Continuous Path）制御を行う。ＣＰ制御では、アーム先端部の制御点を目標まで動作させる際に制御点の目標となる位置及び姿勢，つまり動作軌道が時間関数として設定される。目標となる位置及び姿勢には、教示された位置及び姿勢に加えて、教示された位置及び姿勢に基づいて補間された位置及び姿勢も含まれる。コントローラ３は、ＣＰ制御により、制御点の位置及び姿勢が動作軌道に沿うように、アームにおける各関節の角度を制御する。コントローラ３は、位置及び姿勢の制御において、現在指示されている目標となる位置及び姿勢を実現するための第１軸〜第６軸の角度を算出する逆変換処理を行う。

図２に示すように、ロボット２の各関節には、３次元の直交座標系である第１〜第６座標系Σ₁〜Σ₆が規定されている。各座標系Σ₁〜Σ₆の原点は、第１〜第６軸線Ｊ１〜Ｊ６上の所定位置に定められている。各座標系Σ₁〜Σ₆のｚ軸であるｚ１〜ｚ６軸は、第１〜第６軸線Ｊ１〜Ｊ６と一致している。

ベース４には、ロボット座標系である第０座標系Σ₀が規定されている。第０座標系Σ₀は、第１軸〜第６軸が回転しても変化しない座標系である。本実施形態において、座標系Σ₀の原点は、第１軸線Ｊ１上に定められている。また、座標系Σ₀のｚ軸であるｚ０軸は、第１軸線Ｊ１に一致している。

図２に示すｄ１〜ｄ６，ａ２及びａ３は、以下のように定義される。
ｄ１：第０座標系Σ₀の原点から第１座標系Σ₁の原点までのリンク長
ｄ２：第１座標系Σ₁の原点から第１アーム７の基部までのリンク長
ａ２：第１アーム７の基部から同先端部，第２座標系Σ₂の原点までのリンク長
ｄ３：第２座標系Σ₂の原点から第３オフセットアーム８の先端部までのリンク長
ａ３：第３軸Ｊ３，第４軸Ｊ４の軸心間距離
ｄ４：第３座標系Σ₃の原点から第４座標系Σ₄の原点までのリンク長
ｄ５：第４座標系Σ₄の原点から第５座標系Σ₅の原点までのリンク長
ｄ６：第５座標系Σ₅の原点から第６座標系Σ₆の原点までのリンク長
ａ３については図３を参照。そして、上記ｄ５が第５オフセットアーム１０のリンク長に相当する。

先ず、本実施形態における逆変換処理を説明するための前提として、順変換処理について説明する。
＜順変換処理＞
先ず、ｚ軸回転、ｚ軸移動、ｘ軸移動、ｘ軸回転の順の座標変換で、ＤＨパラメータを表１のように決定した。θ_iが、図２の状態からの各関節の回転角度になる。

ベース座標であるΣ₀からメカニカルインターフェース座標Σ₆までの同次変換行列は次のようになる。ｎ，ｏ，ａは、それぞれノーマルベクトル，オリエントベクトル，アプローチベクトルを示す。尚、表記を簡略化するため、ｓｉｎθ_i，ｃｏｓθ_iそれぞれｓ_i，ｃ_iと表記している。また、例えばＳ₂₃はｓｉｎ（θ₂＋θ₃）を示す。

（８）〜（１０）式を展開すると、（１３）〜（１５）式となる。

また、第５軸，第６軸についてのノーマル，オリエント，アプローチの各ベクトル及び位置座標については以下のようになる。

同次変換行列であるから、ツール座標上での位置ベクトルに左からかけて積をとることで、ベース座標Σ₀での位置ベクトルに変換できる。つまり、関節角とＤＨパラメータとからベース座標での先端位置を求めることができる。姿勢角は、第６軸のノーマル，オリエント，アプローチの各ベクトルｎ₆，ｏ₆，ａ₆で表されている。

＜逆変換処理＞
次に、逆変換処理について説明する。ロボットアームの手先の位置姿勢が、同時変換行列で与えられているとする。図２におけるベース座標Σ₀での位置ベクトルをｐ_iとする。まず、アプローチベクトルａ₆から、手首位置ｐ₅を求める。
ｐ₅＝ｐ₆−ｄ₆ａ₆ …（２０）

従来の６軸ロボットの場合は、手首位置で４，５，６各軸の回転軸が直交する構造であるため、手首位置に対して先に１，２，３軸を解析的に求めることで全ての関節角を求めることができる。すなわち、手首位置は４，５，６各軸の回転軸上にあるので、４，５，６軸の角度を変化させても手首位置は変化しないからである。これに対して本実施形態の６軸ロボット２の場合は、４，５，６軸角度によってｐ₄が変化してしまうため、従来と同様の方法では解くことができない。

そこで、６軸角度を仮定して解く方法を示す。６軸の回転範囲を例えば−１８０度から１８０度とすると、その範囲内で適当な刻み幅で仮定して探索を行う。この回転範囲をSINGLEと称する場合がある。６軸角度が決まれば、（１１），（１２）式からｐ₄を決定できる。
ｐ₄＝ａ₄ｄ₆＋ｐ₆
＝ｏ₆ｄ₆＋ｐ₆
＝（ｏ₆ｓ₆＋ｏ₆ｃ₆）ｄ₆＋ｐ₆ …（２１）

ここで、図４に示すように、ｘｙ軸を１軸角度だけ回転した軸にαｒ座標をとる。ただし、リンクパラメータａ₁＝０としているので、ａ₁≠０の場合には数式が異なる。ｐ₄が決まっているので、１軸角度を用いない式でｐ₄のα座標を表すことができる。またここで、α₁₄≧０の腕形態をLEFTY,α₁₄＜０の腕形態をRIGHTYと定義する。α₁₄が虚数となる場合は、リーチが届かないため解なしとなる。

尚、（２４）式中のｌ₁₄は、αｒ座標の原点からｐ₄までの距離である。よって、軸角度の三角関数を、（２５）〜（２８）式のように求めることができる。

次に、図５に示すように、ロボット２を側面からみたαｚ平面で考える。ｐ₄を満たす２，３軸角度は２組存在する。ｐ₁からｐ₂のアーム長と、ｐ₂からｐ₄のアーム長とから、（２９），（３０）式が成立する。

（３０）式を展開して（２９）式により整理すると、ｐ₂を求めることができる。

ｋ₂＜０の場合はリーチが届かないため解なしとなる。ｚ₁₂も同様に求められる。

（２９）式に（４４），（４５）式を代入すると、（５２）式の等号成立が条件となるため、符号と絶対値を外して下記の複合同順の組み合わせとなる。

図５から下記の式が成立するため、２，３軸の三角関数を求めることができる。
ａｔａｎ２（ｙ，ｘ）を用いれば、１，２，３軸角度を求めることができる。

次に、図６に示すように、アーム形態BELOW,ABOVEを定義した場合、
LEFTYの場合はθ₃＞φ３でABOVE，θ₃＜φ３でBELOWとなる。また、
RIGHTYの場合はθ₃＞φ３でBELOW，θ₃＜φ３でABOVEとなる。ｚ₁₂の符号でも判定可能である。φ３は（６４）式で表される。
φ３＝ａｔａｎ２（ａ₃，ｄ₄） …（６４）

ここまでで１，２，３軸角度が決定し、先端からはｐ４，ｐ５，ｐ６が求められている。これらがｐ４で合流しているため、ここでａ３とａ４とが直交し内積がゼロという条件が成り立たなくてはならない。よって、次の評価式がゼロになる６軸角度であれば、与えられた先端の位置姿勢を満たすことになる。

６軸角度を探索する刻みの都合で評価式がゼロになることは殆どないため、補間によりゼロクロス点を求める。ゼロクロス点として求めた６軸角度で１，２，３軸角度を求め直し、残りの４，５軸角度を決定すればよい。（１８）式から、分母がゼロにならないように注意してｓ５,ｃ５を求める。

同様に、ｓ４，ｃ４を求める。

（＊７）,（＊８）式によりｓｉｎθ₄とｃｏｓθ₄とが求められるので、これにより角度θ₄が求められる。また、先に角度θ₅が求められているので、（＊８）式をｃｏｓθ₅で除算しても良いし、これらに替えて（＊９）,（＊１０）式を用いても良い。

以上が逆変換処理の概略である。次に、本実施形態の作用について図７及び図８を参照して説明する。図７は、コントローラ３が行う逆変換処理の内容を示すフローチャートである。先ず、６軸の動作範囲を計算する（Ｓ１）。図８は、前記動作範囲を説明する図であるが、これはｄ２＝ｄ３である場合を前提としている。ここでは、３軸Ｊ３を、第１アーム７及び第２アーム９を最大限に伸ばした状態での角度に設定し、１軸Ｊ１及び２軸Ｊ２を変化させて、ベース４から計算した４軸位置Ｐ４がとり得る球Ａを求める。次に、６軸Ｊ６の角度を変化させて、図２に示す「フランジ」から計算した４軸位置Ｐ４がとり得る円Ｂを求める。

そして、円Ｂの領域のうち、球Ａの外側にあるものは動作範囲外となり（Ｓ２；ＹＥＳ）、この場合、逆変換処理は失敗となる。円Ｂ∧球Ａであれば（Ｓ２；ＮＯ）６軸角度の初期値θ_6INTを計算し、θ_6Aとして仮決めする（Ｓ３）。初期値θ_6INTは、例えば動作範囲中で取り得る６軸角度の下限値とする。その他、例えばその時点の６軸角度を用いたり、前記角度が動作範囲外にあれば、前記角度から最も近い動作範囲内の角度を採用しても良い。また、円Ｂの中心に設定しても良い。

また、ｄ２≠ｄ３である場合は、以下のように動作範囲内外の判定を行う。ステップＳ２で動作範囲外となるのは、（２４）式又は（４４）式における平方根の内部が負となる場合である。ここで、動作範囲内となるように修正を加えるとすれば、（２４）式のケースでは
ｌ₁₄ ²＝ｒ₁₄ ²
とすれば良い。そこで、Ｌ＝｜ｒ₁₄｜−｜ｌ₁₄｜とすると、
ｗ＝Ｌ／｜ｌ₁₄｜，［ｗｘ₄ ｗｙ₄ ０］^T
だけ位置を補正すれば良い。

また、（４４）式のケースでは

それから、手首形態を決定するための変数ｎにゼロを代入すると（Ｓ４）仮目標位置ｐ_Aを計算する（Ｓ５）。ｐは位置ベクトル［ｘ，ｙ，ｚ］^Tであり、仮目標位置ｐ_Aは、（２１）式より
ｐ＋（ｎｓｉｎθ_6A＋ｏｃｏｓθ_6A）ｄ₅
となる。同時変換行列Ｔ_Aは、回転行列Ｒと目標位置ｐ_Aとから決まる。ここで、仮目標位置ｐ_6Aは、目標位置ｐ₆に対し、
ｐ_6A＝ｐ₆＋（ｎ₆ｓｉｎθ_6A＋ｏ₆ｃｏｓθ_6A）ｄ₅
という関係にある。すなわち、目標位置ｐ₆より、オフセット距離ｄ₅分だけ引いたものが仮目標位置ｐ_6Aとなっている。

尚、変数ｎは、６軸角度の大きさ順に手首形態を採番しておき、後述するように、何番目の手首形態で逆変換処理を確定するかを決定するために使用される。

次に、目標位置ｐ_Aに基づき逆変換処理を行う（Ｓ６）。ここで、
関数ｇ（θ）＝（Ｔ，Ｆ）は、ｄ₅＝０として
角度θ＝［θ₁，θ₂，θ₃，θ₄，θ₅，θ₆］^Tから、同時変換行列Ｔ及びロボット２の形態Ｆを求める関数である。そして、ｇ^-1（Ｔ，Ｆ）は、ｄ₅＝０として同時変換行列Ｔ及び形態Ｆより逆変換して角度θを求める関数である。

次に、逆変換の結果を、評価関数Ｑ（θ，θ_6A）により評価した値ｖを求める（Ｓ７）。ここで、評価関数Ｑとしては、例えば前述した（６５）式により内積を演算する。そして、評価値ｖの絶対値が、ゼロ近傍に設定される閾値ｖ_M以下か否かを判断する（Ｓ８）。前記絶対値が閾値ｖ_Mを超えていれば（ＮＯ）、例えば６軸角度θ_6Aにβ（＞０）を加えて更新する（Ｓ１１）。ここで、角度θ_6Aが動作範囲内となる角度かをチェックし（Ｓ１２）、動作範囲内であれば（ＹＥＳ）ステップＳ５に戻る。角度θ_6Aが動作範囲外であれば（ＮＯ）逆変換処理は失敗となる。

尚、角度θ_6Aは、６軸角度の全範囲内を探索するように変化させる。また、初期値θ_6INTを、動作範囲中で取り得る６軸角度の上限値として、βを減じて更新しても良い。その他、
θ_6A←θ_6A＋α（θ₆−θ_6A），０＜α≦１
のように更新しても良い。また、β，αを繰り返し演算毎に変化させても良いし、黄金分割法やシンプレックス法等の最適化手法を用いても良い。

ここで、図９に示すように、本実施形態のように第３オフセットアーム８を有することでｄ５≠０であるロボットの場合、１つの手先位置に対して最大で４つの手首形態があり、ステップＳ９における評価を満たす６軸角度も最大で４つ存在する。そこで、ステップＳ８において評価値ｖの絶対値が閾値ｖ_M以下になると（ＹＥＳ）変数ｎをインクリメントし（Ｓ９）、変数ｎの値が、予め指定したＮ番目の６軸角度に一致しているか否かを判断する（Ｓ１０）。ｎ＝Ｎであれば（ＹＥＳ）逆変換処理を終了し、ｎ≠Ｎであれば（ＮＯ）ステップＳ１１に移行する。尚、ステップＳ８で（ＹＥＳ）と判断するケースは、逆変換した結果の６軸角度θ₆が、仮決めした角度θ_6Aに略等しくなったことを意味する。

以上のように本実施形態によれば、コントローラ３は、５軸が配置され、４軸と６軸とを連結するリンク長ｄ５の第３オフセットアーム８を有することで、４軸の軸心と６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボット２を制御対象とする。そして、コントローラ３は、アームの先端部である手先を制御点とし、その制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出する。

第６軸の角度を仮決めしてθ_6Aとすると、その角度からオフセットアーム８の方向を求め、第６軸の目標位置Ｐ₆から、リンク長ｄ５を減じた仮目標位置Ｐ_6Aを求めると、仮目標位置Ｐ_Aに対しリンク長ｄ５を「０」に設定して逆変換処理を行う。そして、逆変換処理の結果を評価関数により評価することで、求められた６軸の角度θ₆と仮決めした第６軸の角度θ_6Aとの差が所定値以下となるまで、６軸角度仮決めから処理を繰り返し実行する。このように構成すれば、リンク長ｄ５のオフセットアーム８を有するロボット２についても、逆変換処理を行うことが可能になる。

そして、コントローラ３は、内積ａ３・ａ４を評価関数して用いる。すなわち、逆変換処理が適切に行われていれば、アプローチベクトルａ３とａ４とが直交しておりそれらの内積がゼロになるはずである。したがって、逆変換処理の結果を適切に評価できる。
また、コントローラ３は、同一の手先位置について第６軸の角度を大きさ順に予め採番しておくことで、逆変換処理結果の手首形態を決定するので、手首形態を一意に決定できる。

（第２実施形態）
以下、第１実施形態と同一部分には同一符号を付して説明を省略し、異なる部分について説明する。第２実施形態では、６軸ではなく、第４軸又は第５軸の角度について、大きさの順に手首形態を採番する。すなわち、ｋ（＝４，５）軸角度についてＮ番目の角度を指定する。図１０に示すように、ステップＳ９で「ＹＥＳ」と判断すると、その時点の逆変換結果である角度θをメモリ等に保存する（Ｓ１３）。そして、ステップＳ１２で「ＮＯ」と判断すると、その時点までに保存されていた角度θのうち、ｋ軸角度を、その大きさの順に並び替える（Ｓ１４）。そして、それらのＮ番目の角度を開として採用する（Ｓ１５）。
以上のように第２実施形態によれば、第１実施形態と同様に、逆変換処理結果の手首形態を一意に決定できる。

（第３実施形態）
第３実施形態では、逆変換処理で得られた手首形態を一意に決定するため、ヤコビ行列Ｊを用いる。第１実施形態において、「ステップＳ９における評価を満たす６軸角度は最大で４つ存在する」としたが、これらの境界を分ける点は特異点となるはずであり、ヤコビ行列Ｊは、特異点において行列式がゼロとなる。

すなわち、腕形態RIGHTY,LEFTYと肘形態ABOVE,BELOWに加えて、手首形態FLIP+，FLIP-，NONFLIP+，NONFLIP-の組み合わせがある。そこで、図１１に示すフローチャートでは、ステップＳ９で「ＹＥＳ」と判断するとヤコビ行列Ｊの行列式を計算する（Ｓ１６）。ヤコビ行列Ｊは、（８７）式〜（９９）式で表される。

そして、計算した行列式の符号が、指定した手首形態に合致するか否かを判断する（Ｓ１７）。

以上のように第３実施形態によれば、コントローラ３は、逆変換処理の結果についてヤコビ行列Ｊの行列式を演算する。そして、行列式の符号が予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を一意に決定できる。

本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
コントローラ３を、ロボット２のベース４の外に配置しても良い。
逆変換処理の結果については、その他、逆変換した後に、ｄ５を含めて順変換した結果についての位置誤差がゼロになるか否かを評価しても良い。
また、フランジから求めた５軸の方向Ｚ４Ａと、逆変換で求めた５軸の方向Ｚ４とが一致するか，つまりそれらの内積が「１」となるか否かを評価しても良い。
また、上記５軸の方向Ｚ４Ａと、逆変換で求めた４軸の方向Ｚ３とが直交するか，つまりそれらの内積が「０」となるか否かを評価しても良い。

繰り返し演算については、動作範囲を複数に分割し、分割した各角度範囲について評価値を求めても良い。
また、動作範囲を分割した場合、評価値より求められた解の近辺で、求めた評価値から評価関数の近似関数を求め、その近似関数を用いて確からしい角度を求めても良い。

図面中、１はロボットシステム、２はロボット、３はコントローラ、４はベース、５はショルダ、６は第２オフセットアーム、７は第１アーム、８は第３オフセットアーム、９は第２アーム、１０は第５オフセットアーム、１１は手首、１２はハンドを示す。

Claims (8)

1. 第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
   第６軸の角度を仮決めする角度仮決め部と、
   仮決めした第６軸の角度から、前記オフセットアームの方向を求める方向算出部と、
   第６軸の目標位置から、リンク長ｄ５を減じた仮目標位置を求める仮目標位置算出部と、
   前記仮目標位置に対し、リンク長ｄ５を「０」に設定して逆変換処理を行う逆変換処理部と、
   前記逆変換処理の結果を評価関数により評価することで、求められた第６軸の角度と、仮決めした第６軸の角度との差が所定値以下となるまで前記角度仮決め部による仮決めから処理を繰り返し実行する評価部とを備えるロボットの制御装置。
2. 第３軸，第４軸のアプローチベクトルをそれぞれａ３，ａ４とすると、
   前記評価部は、内積ａ３・ａ４を前記評価関数とする請求項１記載のロボットの制御装置。
3. 同一の手先位置について、第４軸から第６軸の何れかの複数の角度を予め採番しておくことで、逆変換処理結果の手首形態を決定する形態決定部を備える請求項１又は２記載のロボットの制御装置。
4. 逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項１又は２記載のロボットの制御装置。
5. 第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
   第６軸の角度を仮決めするステップと、
   仮決めした第６軸の角度から、前記オフセットアームの方向を求めるステップと、
   第６軸の目標位置から、リンク長ｄ５を減じた仮目標位置を求めるステップと、
   前記仮目標位置に対し、リンク長ｄ５を「０」に設定して逆変換処理を行うステップと、
   前記逆変換処理の結果を評価関数により評価することで、求められた第６軸の角度と、仮決めした第６軸の角度との差が所定値以下となるまで前記仮決めするステップから処理を繰り返し実行するロボットの逆変換処理方法。
6. 第３軸，第４軸のアプローチベクトルをそれぞれａ３，ａ４とすると、
   内積ａ３・ａ４を前記評価関数とする請求項５記載のロボットの逆変換処理方法。
7. 同一の手先位置について、第４軸から第６軸の何れかの複数の角度を予め採番しておくことで、逆変換処理結果の手首形態を決定する請求項５又は６記載のロボットの逆変換処理方法。
8. 逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項５又は６記載のロボットの逆変換処理方法。

Images