(57)【要約】【課題】オフセットアームを備えるロボットについても、逆変換処理を行うことができるロボットの制御装置を提供する。【解決手段】５軸が配置され、４軸と６軸とを連結するリンク長ｄ５の第３オフセットアーム８を有し、４軸の軸心と６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボット２を制御対象とする。アームの先端部である手先を制御点とし、その制御点の目標となる位置及び姿勢を逆変換処理して各軸の角度を算出する。第６軸の位置を仮決めし、仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めすると、位置Ｐ５’及びＰ６’がロボット２のリンクパラメータに基づく動作範囲内にあるか否かを判定し、動作範囲内にあればそれらの位置に基づく同次変換行列についてリンク長ｄ５を「０」に設定して逆変換処理を行うと、逆変換処理により求められた各軸の角度を用いて順変換処理を行う。【選択図】図７
2019093488.tif 000002
【特許請求の範囲】
【請求項１】
第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めする位置仮決め部と、
前記位置Ｐ５’及びＰ６’が、前記ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定する動作範囲判定部と、
前記位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う逆変換処理部と、
前記逆変換処理により求められた各軸の角度を用いて順変換処理を行う順変換処理部と、
前記制御点の目標位置と、前記順変換処理により得られた位置との差の位置行列ｐEを求めると共に、前記目標位置に対応する回転行列に、前記順変換処理により得られた回転行列の逆行列を乗じて回転行列ＲEを求め評価値演算部と、
前記位置行列ｐEのノルム||ｐE||が閾値を超えているか、又は前記回転行列ＲEより求められる角度||ＲE||が閾値を超えていると、前記回転行列ＲEの回転軸ベクトルｑEを求め、
前記角度||ＲE||と単位回転軸ベクトルｑE／||ｑE||との積である回転ベクトルｒEを求め、
前記位置行列ｐEと前記回転ベクトルｒEとから定まる位置行列ＰEに対し、前記逆変換処理により求められた各軸の角度から求めたヤコビ行列の逆行列を乗じた結果によって前記角度を更新し、前記順変換処理からの処理を繰り返し実行する繰返し演算実行部とを備えるロボットの制御装置。
【請求項２】
前記逆変換処理部は、前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する請求項１記載のロボットの制御装置。
【請求項３】
前記逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項１又は２記載のロボットの制御装置。
【請求項４】
第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めするステップと、
前記位置Ｐ５’及びＰ６’が、前記ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定するステップと、
前記位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行うステップと、
前記逆変換処理により求められた各軸の角度を用いて順変換処理を行うステップと、
前記制御点の目標位置と、前記順変換処理により得られた位置との差の位置行列ｐEを求めると共に、前記目標位置に対応する回転行列に、前記順変換処理により得られた回転行列の逆行列を乗じて回転行列ＲEを求めるステップと、
前記位置行列ｐEのノルム||ｐE||が閾値を超えているか、又は前記回転行列ＲEより求められる角度||ＲE||が閾値を超えていると、前記回転行列ＲEの回転軸ベクトルｑEを求め、
前記角度||ＲE||と単位回転軸ベクトルｑE／||ｑE||との積である回転ベクトルｒEを求め、
前記位置行列ｐEと前記回転ベクトルｒEとから定まる位置行列ＰEに対し、前記逆変換処理により求められた各軸の角度から求めたヤコビ行列の逆行列を乗じた結果によって前記角度を更新し、前記順変換処理を行うステップから繰り返し実行するロボットの逆変換処理方法。
【請求項５】
前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する請求項４記載のロボットの逆変換処理方法。
【請求項６】
逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項４又は５記載のロボットの逆変換処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、第４軸と第６軸とを連結するオフセットアームを有するロボットの動作を制御する装置,及び手先位置を逆変換処理する方法に関する。
【背景技術】
【０００２】
一般的な垂直６軸型のロボットアームは、第４軸〜第６軸の各軸心が一点で交差する構造となっている。このようなロボットアームでは、第５軸を回転させて第６軸を含む手首部分を変位させるため、アームの先端部において手首を保持している部分に隙間が形成されている。そのような隙間があると、異物を挟み込んでしまう可能性が有る。そこで、第４軸と第６軸とを連結するオフセットアームを備えることで、手首が変位する部分の隙間をなくした形態の産業用ロボットが、例えば意匠登録１５８３７５５等に開示されている。この形態では、オフセットアームの存在により、第４軸の軸心と第６軸の軸心とが交差することなく互いに平行な関係となっている。
【０００３】
このような形態のアームについて、手先の位置・姿勢から各軸の角度を求める逆変換処理を行うことを想定すると、従来の手法では困難である。例えば特許文献１に開示されているのは、ロボット組み付け時の誤差を含む場合でもＤＨパラメータに基づく逆変換処理を行うことを可能にした技術である。特許文献１では、オフセットアームの長さをｄ５とすると、ｄ５＝０であることを前提に、前記誤差に対応する微小領域を仮定して線形化することで収束演算を行っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１７−６１０２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ｄ５≠０のロボットに特許文献１の手法を適用することを想定すると、収束演算の初期誤差としてのｄ５が大きな値となるため、演算の繰り返し回数が膨大になったり、演算が収束せずに発散する可能性が有る。また、オフセット分があることで、本来はリーチが届く範囲であってもオフセット分が正しく反映されず、リーチが届かないと誤判定される可能性がある。
【０００６】
本発明は、上記実情に鑑みてなされたものであり、その目的は、オフセットアームを備えるロボットについても、逆変換処理を行うことができるロボットの制御装置,及びロボットの逆変換処理方法を提供することにある。
【課題を解決するための手段】
【０００７】
請求項１記載のロボットの制御装置は、第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットを制御対象とする。そして、アームの先端部を制御点とし、その制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出する。
【０００８】
位置仮決め部は、第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めする。動作範囲判定部は、位置Ｐ５’及びＰ６’が、ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定する。逆変換処理部は、位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う。また、順変換処理部は、逆変換処理により求められた各軸の角度を用いて順変換処理を行う。
【０００９】
評価値演算部は、制御点の目標位置と、順変換処理により得られた位置との差の位置行列ｐEを求めると共に、目標位置に対応する回転行列に、順変換処理により得られた回転行列の逆行列を乗じて回転行列ＲEを求める。そして、繰返し演算実行部は、位置行列ｐEのノルム||ｐE||が閾値を超えているか、又は前記回転行列ＲEより求められる角度||ＲE||が閾値を超えていると、前記回転行列ＲEの回転軸ベクトルｑEを求める。また、前記角度||ＲE||と単位回転軸ベクトルｑE／||ｑE||との積である回転ベクトルｒEを求める。そして、位置行列ｐEと回転ベクトルｒEとから定まる位置行列ＰEに対し、逆変換処理により求められた各軸の角度から求めたヤコビ行列の逆行列を乗じた結果によって前記角度を更新し、順変換処理から繰り返し実行する。
このように構成すれば、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有するロボットについても、第６軸の角度を仮決めして逆変換処理を行い、その処理結果を順変換処理して得られる行列ｐE及び行列ＲEを評価して演算を収束させることで、逆変換処理を行うことが可能になる。
【００１０】
請求項２記載のロボットの制御装置によれば、逆変換処理部は、前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する。これにより、逆変換処理を継続して実行できる。
【００１１】
請求項３記載のロボットの制御装置によれば、形態決定部は、逆変換処理の結果についてヤコビ行列の行列式を演算する。そして、行列式の符号が予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する。例えば、各軸の可動範囲が−１８０度〜１８０度である場合に、５軸を動かして手首形態を目視で確認すると、最大で２つの腕形態,２つの肘形態及び２つの手首形態の組み合わせとなることが分かる。
そして、２つの手首形態の境界を分ける点は特異点となるはずであり、ヤコビ行列は、特異点において行列式がゼロとなる。そこで、各形態の組み合わせについて、逆変換処理結果の手首形態とヤコビ行列の行列式の符号とが対応するか否かにより、その形態が指定したものとなっているか否かを判定できる。すなわち、４軸の角度が−９０度〜９０度，−１８０度〜−９０度又は９０度〜１８０度の何れに属するかと組み合わせることで、最大で４つの手首形態を判別できる。
【図面の簡単な説明】
【００１２】
【図１】第１実施形態であり、ロボットシステムの構成を示す図
【図２】ロボットの座標系を示す図
【図３】ロボットの構成を、ｘｚ平面及びｙｚ平面で示す図
【図４】ロボットのアーム形態をｘｙ座標及びαｒ座標で示す図
【図５】ロボットのアーム形態をαｚ座標で示す図
【図６】αｚ座標で定義したアームの各形態を示す図
【図７】コントローラが行う逆変換処理を示すフローチャート
【図８】ｄ２＝ｄ３である場合の動作範囲を説明する図
【図９】同一の手先位置についてロボットアームが取り得る４つの手首形態を説明する図
【発明を実施するための形態】
【００１３】
以下、一実施形態について図１から図９を参照して説明する。図１に示すように、ロボットシステム１は、垂直多関節型のロボット２、ロボット２を制御するコントローラ３をベース４の内部に備えている。このロボットシステム１は、一般的な産業用に用いられている。ロボット２は、いわゆる６軸の垂直多関節型ロボットである。ベース４上に、Ｚ方向の軸心を持つ第１軸；Ｊ１を介してショルダ５が水平方向に回転可能に連結されている。ショルダ５には、Ｙ方向の軸心を持つ第２軸；Ｊ２を備え、Ｙ方向に伸びる第２オフセットアーム６を介して、上方に延びる第１アーム７の下端部が垂直方向に回転可能に連結されている。第１アーム７の先端部には、Ｙ方向の軸心を持つ第３軸；Ｊ３を備え、−Ｙ方向に伸びる第３オフセットアーム８を介して、第２アーム９が垂直方向に回転可能に連結されている。第２アーム９は、基部９ａ及び先端部９ｂからなる。
【００１４】
第２アーム９は、Ｘ方向の軸心を持つ第４軸；Ｊ４を備え、基部９ａに対して先端部９ｂが捻り回転可能に連結されている。第２アーム９の先端部には、Ｙ方向の軸心を持つ第５軸；Ｊ５を備え、−Ｙ方向に伸びる第５オフセットアーム１０を介して、手首１１が垂直方向に回転可能に連結されている。手首１１には、Ｘ方向の軸心を持つ第６軸；Ｊ６を介して、図２に示すフランジ及びハンド１２が捻り回転可能に連結されている。ロボット２に設けられている各軸；Ｊ１〜Ｊ６には、それぞれに対応して駆動源となる図示しないモータが設けられている。
【００１５】
コントローラ３は、ロボット２の制御装置であり、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御手段においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりそれぞれのモータを駆動する。
【００１６】
コントローラ３は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット２のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。コントローラ３は、角度仮決め部,方向算出部,仮目標位置算出部,逆変換処理部,評価部及び形態決定部に相当する。位置検出回路には、ロボット２の各関節に設けられた図示しない各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。
【００１７】
コントローラ３は、予め設定された動作プログラムを実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置及び姿勢を制御する。本実施形態において、コントローラ３は、ＣＰ（Continuous Path）制御を行う。ＣＰ制御では、アーム先端部の制御点を目標まで動作させる際に制御点の目標となる位置及び姿勢，つまり動作軌道が時間関数として設定される。目標となる位置及び姿勢には、教示された位置及び姿勢に加えて、教示された位置及び姿勢に基づいて補間された位置及び姿勢も含まれる。コントローラ３は、ＣＰ制御により、制御点の位置及び姿勢が動作軌道に沿うように、アームにおける各関節の角度を制御する。コントローラ３は、位置及び姿勢の制御において、現在指示されている目標となる位置及び姿勢を実現するための第１軸〜第６軸の角度を算出する逆変換処理を行う。
【００１８】
図２に示すように、ロボット２の各関節には、３次元の直交座標系である第１〜第６座標系Σ1〜Σ6が規定されている。各座標系Σ1〜Σ6の原点は、第１〜第６軸線Ｊ１〜Ｊ６上の所定位置に定められている。各座標系Σ1〜Σ6のｚ軸であるＺ１〜Ｚ６軸は、第１〜第６軸線Ｊ１〜Ｊ６と一致している。
【００１９】
ベース４には、ロボット座標系である第０座標系Σ0が規定されている。第０座標系Σ0は、第１軸〜第６軸が回転しても変化しない座標系である。本実施形態において、座標系Σ0の原点は、第１軸線Ｊ１上に定められている。また、座標系Σ0のｚ軸であるＺ０軸は、第１軸線Ｊ１に一致している。
【００２０】
図２に示すｄ１〜ｄ６，ａ２及びａ３は、以下のように定義される。
ｄ１：第０座標系Σ0の原点から第１座標系Σ1の原点までのリンク長
ｄ２：第１座標系Σ1の原点から第１アーム７の基部までのリンク長
ａ２：第１アーム７の基部から同先端部，第２座標系Σ2の原点までのリンク長
ｄ３：第２座標系Σ2の原点から第３オフセットアーム８の先端部までのリンク長
ａ３：第３軸Ｊ３，第４軸Ｊ４の軸心間距離
ｄ４：第３座標系Σ3の原点から第４座標系Σ4の原点までのリンク長
ｄ５：第４座標系Σ4の原点から第５座標系Σ5の原点までのリンク長
ｄ６：第５座標系Σ5の原点から第６座標系Σ6の原点までのリンク長
ａ３については図３を参照。そして、上記ｄ５が第５オフセットアーム１０のリンク長に相当する。
【００２１】
先ず、本実施形態における逆変換処理を説明するための前提として、順変換処理について説明する。
＜順変換処理＞
先ず、ｚ軸回転、ｚ軸移動、ｘ軸移動、ｘ軸回転の順の座標変換で、ＤＨパラメータを表１のように決定した。θiが、図２の状態からの各関節の回転角度になる。
【００２２】
【表１】
2019093488.tif 000003
【００２３】
ベース座標であるΣ0からメカニカルインターフェース座標Σ6までの同次変換行列は次のようになる。ｎ，ｏ，ａは、それぞれノーマルベクトル，オリエントベクトル，アプローチベクトルを示す。尚、表記を簡略化するため、ｓｉｎθi，ｃｏｓθiそれぞれｓi，ｃiと表記している。また、例えばＳ23はｓｉｎ（θ2＋θ3）を示す。
【００２４】
【数１】
2019093488.tif 000004
【００２５】
【数２】
2019093488.tif 000005
【００２６】
（８）〜（１０）式を展開すると、（１３）〜（１５）式となる。
【００２７】
【数３】
2019093488.tif 000006
【００２８】
また、第５軸，第６軸についてのノーマル，オリエント，アプローチの各ベクトル及び位置座標については以下のようになる。
【００２９】
【数４】
2019093488.tif 000007
【００３０】
同次変換行列であるから、ツール座標上での位置ベクトルに左からかけて積をとることで、ベース座標Σ0での位置ベクトルに変換できる。つまり、関節角とＤＨパラメータとからベース座標での先端位置を求めることができる。姿勢角は、第６軸のノーマル，オリエント，アプローチの各ベクトルｎ6，ｏ6，ａ6で表されている。
【００３１】
＜逆変換処理＞
次に、逆変換処理について説明する。ロボットアームの手先の位置姿勢が、同時変換行列で与えられているとする。図２におけるベース座標Σ0での位置ベクトルをｐiとする。まず、アプローチベクトルａ6から、手首位置ｐ5を求める。
ｐ5＝ｐ6−ｄ6ａ6                  …（２０）
【００３２】
従来の６軸ロボットの場合は、手首位置で４，５，６各軸の回転軸が直交する構造であるため、手首位置に対して先に１，２，３軸を解析的に求めることで全ての関節角を求めることができる。すなわち、手首位置は４，５，６各軸の回転軸上にあるので、４，５，６軸の角度を変化させても手首位置は変化しないからである。これに対して本実施形態の６軸ロボット２の場合は、４，５，６軸角度によってｐ4が変化してしまうため、従来と同様の方法では解くことができない。
【００３３】
そこで、６軸角度を仮定して解く方法を示す。６軸の回転範囲を例えば−１８０度から１８０度とすると、その範囲内で適当な刻み幅で仮定して探索を行う。この回転範囲をSINGLEと称する場合がある。６軸角度が決まれば、（１１），（１２）式からｐ4を決定できる。
ｐ4＝ａ4ｄ6＋ｐ6
＝ｏ6ｄ6＋ｐ6
＝（ｏ6ｓ6＋ｏ6ｃ6）ｄ6＋ｐ6            …（２１）
【００３４】
ここで、図４に示すように、ｘｙ軸を１軸角度だけ回転した軸にαｒ座標をとる。ただし、リンクパラメータａ1＝０としているので、ａ1≠０の場合には数式が異なる。ｐ4が決まっているので、１軸角度を用いない式でｐ4のα座標を表すことができる。またここで、α14≧０の腕形態をLEFTY,α14＜０の腕形態をRIGHTYと定義する。α14が虚数となる場合は、リーチが届かないため解なしとなる。
【００３５】
【数５】
2019093488.tif 000008
【００３６】
尚、（２４）式中のｌ14は、αｒ座標の原点からｐ4までの距離である。
よって、軸角度の三角関数を、（２５）〜（２８）式のように求めることができる。
【００３７】
【数６】
2019093488.tif 000009
【００３８】
次に、図５に示すように、ロボット２を側面からみたαｚ平面で考える。ｐ4を満たす２，３軸角度は２組存在する。ｐ1からｐ2のアーム長と、ｐ2からｐ4のアーム長とから、（２９），（３０）式が成立する。
【００３９】
【数７】
2019093488.tif 000010
【００４０】
（３０）式を展開して（２９）式により整理すると、ｐ2を求めることができる。
【００４１】
【数８】
2019093488.tif 000011
【００４２】
ｋ2＜０の場合はリーチが届かないため解なしとなる。ｚ12も同様に求められる。
【数９】
2019093488.tif 000012
【００４３】
（２９）式に（４４），（４５）式を代入すると、（５２）式の等号成立が条件となるため、符号と絶対値を外して下記の複合同順の組み合わせとなる。
【００４４】
【数１０】
2019093488.tif 000013
【００４５】
図５から下記の式が成立するため、２，３軸の三角関数を求めることができる。
ａｔａｎ２（ｙ，ｘ）を用いれば、１，２，３軸角度を求めることができる。
【００４６】
【数１１】
2019093488.tif 000014
【００４７】
次に、図６に示すように、アーム形態BELOW,ABOVEを定義した場合、
LEFTYの場合はθ3＞φ３でABOVE，θ3＜φ３でBELOWとなる。また、
RIGHTYの場合はθ3＞φ３でBELOW，θ3＜φ３でABOVEとなる。ｚ12の符号でも判定可能である。φ３は（６４）式で表される。
φ３＝ａｔａｎ２（ａ3，ｄ4）            …（６４）
【００４８】
次に、角度θ5，角度θ6を求める。
【００４９】
【数１２】
2019093488.tif 000015
【００５０】
（＊４）式より、角度θ5が求められる。また、（＊４）式と（＊１），（＊２）式よりｓｉｎθ6とｃｏｓθ6とが求められるので、これにより角度θ6が求められる。
【００５１】
【数１３】
2019093488.tif 000016
【００５２】
（＊７）,（＊８）式によりｓｉｎθ4とｃｏｓθ4とが求められるので、これにより角度θ4が求められる。尚、（＊４）式に±の複号があるが、これについては、手首形態の指定において４軸角度又は５軸角度の条件を満たす符号を採用する。また、先に角度θ5が求められているので、（＊８）式をｃｏｓθ5で除算しても良いし、これらに替えて（＊９）,（＊１０）式を用いても良い。
【００５３】
【数１４】
2019093488.tif 000017
【００５４】
以上が逆変換処理の概略である。次に、本実施形態の作用について図７及び図８を参照して説明する。図７は、コントローラ３が行う逆変換処理の内容を示すフローチャートである。先ず、６軸の位置を仮決めしてＰ６’とすると、その位置Ｐ６’から５軸の位置を仮決めしてＰ５’とする。そして、これらを用いて同次変換行列Ｔ’を決定する（Ｓ１）。そして、仮決めした位置Ｐ５’が動作範囲内にあるか否かを判断する（Ｓ２）。
【００５５】
図８は、前記動作範囲を説明する図であるが、これはｄ２＝ｄ３である場合を前提としている。半径Ｌ１の球Ａと、半径（Ｌ１＋Ｌ２）の球Ｂとを設定する。Ｌ１，Ｌ２は、それぞれ（＊１１）式，（＊１２）式で定義される。
【数１５】
2019093488.tif 000018
【００５６】
そして、位置Ｐ５’が球Ａの外にあれば（Ｓ２；動作範囲外）、同次変換行列Ｔ’で決まる位置から動作範囲の境界までの距離をＬとする（Ｓ９）。続いて、距離Ｌがリンク長ｄ５以下か否かを判断する（Ｓ１０）。これは、仮決めした位置Ｐ６’が動作範囲内にあるか否かの判断である。距離Ｌがリンク長ｄ５を超えていれば（ＮＯ）エラーとして処理を終了する。
【００５７】
一方、距離Ｌがリンク長ｄ５以下であれば（ＹＥＳ）、位置Ｐ５’を球Ａの内側に移動させるように修正する（Ｓ１１）。例えば、位置Ｐ５’を、位置Ｐ１と位置Ｐ５’とを結ぶ直線と、球Ａとの交点に移動させれば良い。位置Ｐ５’を移動させることに伴い、位置Ｐ６’も移動する。尚、ステップＳ１０では、Ｌ２＝ｄ５として、位置Ｐ５’が球Ｂ外か否かを判断しても良い。
【００５８】
また、ｄ２≠ｄ３である場合は、以下のように動作範囲内外の判定を行う。ステップＳ２で動作範囲外となるのは、（２４）式又は（４４）式における平方根の内部が負となる場合である。ここで、動作範囲内となるように修正を加えるとすれば、（２４）式のケースでは
ｌ142＝ｒ142
とすれば良い。そこで、Ｌ＝｜ｒ14｜−｜ｌ14｜とすると、
ｗ＝Ｌ／｜ｌ14｜，［ｗｘ4  ｗｙ4  ０］T
だけ位置を補正すれば良い。
【００５９】
また、（４４）式のケースでは
【００６０】
【数１６】
2019093488.tif 000019
【００６１】
ステップＳ２で「動作範囲内」と判断すると、またステップＳ１１を実行すると、逆変換処理を行う（Ｓ３）。ここで、ｇ-1（Ｔ’，Ｆ）は、ｄ5＝０として同時変換行列Ｔ’，ロボット２の形態Ｆから角度θ’＝［θ1’，θ2’，θ3’，θ4’，θ5’，θ6’］Tを求める関数である。
次に、逆変換の結果として求められた角度θ’を用いて順変換処理を行う（Ｓ４）。この順変換処理の結果得られた同時変換行列をＴ”，形態をＦ”とする。同時変換行列Ｔ”は、要素として回転行列Ｒ”，位置行列ｐ”を含む。
【００６２】
次に、目標位置ｐと上記位置ｐ”との差を求め、位置誤差ベクトルｐEを求める。また、目標位置ｐに対応する回転行列Ｒに回転行列Ｒ”の逆行列を乗じて評価用の回転行列ＲEを求める（Ｓ５）。そして、ベクトルｐEのノルム||ｐE||が閾値ｐEM以下か（Ｓ６）、また、回転行列ＲEの角度||ＲE||が閾値ＲEM以下かを（Ｓ７）それぞれ判断する。ここで、||ｐE||，||ＲE||は、それぞれ（＊１３），（＊１４）式で定義される。
【００６３】
【数１７】
2019093488.tif 000020
【００６４】
ステップＳ７で「ＹＥＳ」と判断すると、位置ｐ”と回転行列Ｒ”より決まる手首形態が指定した形態か否かを判断し（Ｓ８）、指定した形態が得られていれば逆変換処理を終了する。一方、ステップＳ６又はＳ７で「ＮＯ」と判断すると、逆変換処理及び順変換処理の繰り返し実行回数が予め定めた規定数を超えたか否かを判断する（Ｓ１２）。ここで、規定数を超えると（ＹＥＳ）エラーとして処理を終了する。
【００６５】
規定数を超えていなければ（Ｓ１２；ＮＯ）、回転行列ＲEの要素であるノーマル，オリエント，アプローチの各ベクトルから、次式により回転軸ベクトルｑEを求める（Ｓ１３）。
【００６６】
【数１８】
2019093488.tif 000021
【００６７】
次に、回転行列ＲEの角度||ＲE||と、単位回転軸ベクトルｑE／||ｑE||との積である回転ベクトルｒEを、次式により求める（Ｓ１４）。
【００６８】
【数１９】
2019093488.tif 000022
【００６９】
それから、位置行列ｐEと回転ベクトルｒEとから定まる位置行列ＰEに対し、逆変換処理により求められた各軸の角度θから求めたヤコビ行列Ｊの逆行列を乗じた結果によって角度θ’を更新すると（Ｓ１５）、ステップＳ４に移行する。ヤコビ行列Ｊは、（８７）式〜（９９）式で表される。
【００７０】
【数２０】
2019093488.tif 000023
【００７１】
【数２１】
2019093488.tif 000024
【００７２】
【数２２】
2019093488.tif 000025
【００７３】
次に、ステップＳ８の処理判定について説明する。図９に示すように、本実施形態のように第３オフセットアーム８を有することでｄ５≠０であるロボットの場合、１つの手先位置に対して最大で４つの手首形態があり、ステップＳ９における評価を満たす６軸角度も最大で４つ存在する。そこで、ステップＳ８において、逆変換処理で得られた手首形態を一意に決定するため、ヤコビ行列Ｊを用いる。上記の最大で４つ存在する６軸角度は、これらの境界を分ける点は特異点となるはずであり、ヤコビ行列Ｊは、特異点において行列式がゼロとなる。
【００７４】
ヤコビ行列の符号と、従来の手首形態の考え方，４軸角度が−９０度〜９０度，−１８０度〜−９０度又は９０度〜１８０度の何れになるかの組み合わせで、最大で４つ存在する手首形態FLIP+,FLIP-,NONFLIP+,NONFLIP-を判別することができる。
【００７５】
すなわち、腕形態RIGHTY,LEFTYと肘形態ABOVE,BELOWに加えて、手首形態FLIP+,FLIP-,NONFLIP+,NONFLIP-の組み合わせがある。そこで、ステップＳ８ではヤコビ行列Ｊの行列式を計算する。
【００７６】
そして、計算した行列式の符号が、指定した手首形態に合致するか否かを判断する。
【００７７】
以上のように本実施形態によれば、コントローラ３は、５軸が配置され、４軸と６軸とを連結するリンク長ｄ５の第３オフセットアーム８を有することで、４軸の軸心と６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボット２を制御対象とする。そして、コントローラ３は、アームの先端部である手先を制御点とし、その制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出する。
【００７８】
また、コントローラ３は、第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めすると、位置Ｐ５’及びＰ６’が、ロボット２のリンクパラメータに基づく動作範囲内にあるか否かを判定する。位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う。それから、逆変換処理により求められた各軸の角度を用いて順変換処理を行う。
【００７９】
更に、制御点の目標位置と、順変換処理により得られた位置との差の位置誤差ベクトルｐEを求めると共に、目標位置に対応する回転行列に、順変換処理により得られた回転行列の逆行列を乗じて回転行列ＲEを求める。そして、位置誤差ベクトルｐEのノルム||ｐE||が閾値を超えているか、又は回転行列ＲEより求められる角度||ＲE||が閾値を超えていると、回転行列ＲEの要素であるノーマル，オリエント，アプローチの各ベクトルから回転軸ベクトルｑEを求め、角度||ＲE||と単位回転軸ベクトルｑE／||ｑE||との積である回転ベクトルｒEを求める。それから、位置行列ｐEと回転ベクトルｒEとから定まる位置行列ＰEに対し、逆変換処理により求められた各軸の角度θから求めたヤコビ行列Ｊの逆行列を乗じた結果によって角度θ’を更新すると、順変換からの処理を繰り返し実行する。
【００８０】
このように構成すれば、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有するロボット２についても、第６軸の角度を仮決めして逆変換処理を行い、その処理結果を順変換処理して得られる行列ｐE及び行列ＲEを評価して演算を収束させることで、逆変換処理を行うことが可能になる。
【００８１】
また、コントローラ３は、位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように位置Ｐ５’及びＰ６’を修正する。同次変換行列Ｔ’により決まる制御点の位置についても同様に、動作範囲内にあるか否かを判断し、動作範囲内になければ動作範囲内に位置するように同次変換行列Ｔ’を修正する。これにより、逆変換処理や繰返し演算を継続して実行できる。
【００８２】
本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
コントローラ３を、ロボット２のベース４の外に配置しても良い。
【符号の説明】
【００８３】
図面中、１はロボットシステム、２はロボット、３はコントローラ、４はベース、５はショルダ、６は第２オフセットアーム、７は第１アーム、８は第３オフセットアーム、９は第２アーム、１０は第５オフセットアーム、１１は手首、１２はハンドを示す。
【図１】
2019093488.tif 000026
【図２】
2019093488.tif 000027
【図３】
2019093488.tif 000028
【図４】
2019093488.tif 000029
【図５】
2019093488.tif 000030
【図６】
2019093488.tif 000031
【図７】
2019093488.tif 000032
【図８】
2019093488.tif 000033
【図９】
2019093488.tif 000034