特許 6891773 ロボットの制御装置及びロボットの逆変換処理方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6891773

(24)【登録日】2021年5月31日

(45)【発行日】2021年6月18日

(54)【発明の名称】ロボットの制御装置及びロボットの逆変換処理方法

(51)【国際特許分類】

   B25J 9/10 20060101AFI20210607BHJP

【ＦＩ】

   B25J9/10 A

【請求項の数】8

【全頁数】23

(21)【出願番号】特願2017-225910(P2017-225910)

(22)【出願日】2017年11月24日

(65)【公開番号】特開2019-93487(P2019-93487A)

(43)【公開日】2019年6月20日

【審査請求日】2020年8月20日

(73)【特許権者】

【識別番号】501428545

【氏名又は名称】株式会社デンソーウェーブ

(74)【代理人】

【識別番号】110000567

【氏名又は名称】特許業務法人 サトー国際特許事務所

(72)【発明者】

【氏名】川瀬 大介

(72)【発明者】

【氏名】佐藤 晶則

(72)【発明者】

【氏名】河地 勇登

【審査官】 臼井 卓巳

(56)【参考文献】

【文献】 特開昭６２−１９３７８６（ＪＰ，Ａ）

【文献】 特開昭６４−００８４０７（ＪＰ，Ａ）

【文献】 特開２０１３−１３２７３１（ＪＰ，Ａ）

【文献】 特開２０１３−１６６２２２（ＪＰ，Ａ）

【文献】 特開２０１７−０６１０２２（ＪＰ，Ａ）

【文献】 米国特許第０４６８０５１９（ＵＳ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ ９／１０−１３／００

Ｇ０５Ｂ １３／０２−１９／１８

(57)【特許請求の範囲】

【請求項1】

第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めする位置仮決め部と、
前記位置Ｐ５’及びＰ６’が、前記ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定する動作範囲判定部と、
前記位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う逆変換処理部と、
前記逆変換処理により求められた各軸の角度を用いて順変換処理を行う順変換処理部と、
前記制御点の目標位置と、前記順変換処理により得られた位置との差の位置行列ｐ_Eを求めると共に、前記目標位置に対応する回転行列に、前記順変換処理により得られた回転行列の逆行列を乗じて回転行列Ｒ_Eを求め評価値演算部と、
前記位置行列ｐ_Eのノルムが閾値を超えているか、又は前記回転行列Ｒ_Eより求められる角度が閾値を超えていると、前記位置行列ｐ_E及び前記回転行列Ｒ_Eを反映させた同時変換行列により前記逆変換処理からの処理を繰り返し実行する繰返し演算実行部とを備えるロボットの制御装置。

【請求項2】

前記逆変換処理部は、前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する請求項１記載のロボットの制御装置。

【請求項3】

前記繰返し演算実行部は、前記同次変換行列により決まる前記制御点の位置が、前記動作範囲内にあるか否かを判断し、前記制御点の位置が前記動作範囲内になければ、当該動作範囲内に位置するように前記同次変換行列を修正する請求項１又は２記載のロボットの制御装置。

【請求項4】

前記逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項１から３の何れか一項に記載のロボットの制御装置。

【請求項5】

第５軸が配置され、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有することで、第４軸の軸心と第６軸の軸心とが平行となる構造の垂直６軸型のアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を逆変換処理することで各軸の角度を算出するもので、
第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めするステップと、
前記位置Ｐ５’及びＰ６’が、前記ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定するステップと、
前記位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行うステップと、
前記逆変換処理により求められた各軸の角度を用いて順変換処理を行うステップと、
前記制御点の目標位置と、前記順変換処理により得られた位置との差の位置行列ｐ_Eを求めると共に、前記目標位置に対応する回転行列に、前記順変換処理により得られた回転行列の逆行列を乗じて回転行列Ｒ_Eを求めるステップと、
前記位置行列ｐ_Eのノルムが閾値を超えているか、又は前記回転行列Ｒ_Eより求められる角度が閾値を超えていると、前記位置行列ｐ_E及び前記回転行列Ｒ_Eを反映させた同時変換行列により前記逆変換処理を行うステップから繰り返し実行するロボットの逆変換処理方法。

【請求項6】

前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する請求項５記載のロボットの逆変換処理方法。

【請求項7】

前記同次変換行列により決まる前記制御点の位置が、前記動作範囲内にあるか否かを判断し、前記制御点の位置が前記動作範囲内になければ、当該動作範囲内に位置するように前記同次変換行列を修正する請求項５又は６記載のロボットの逆変換処理方法。

【請求項8】

逆変換処理の結果についてヤコビ行列の行列式を演算し、その行列式の符号が、予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を決定する形態決定部を備える請求項５から７の何れか一項に記載のロボットの逆変換処理方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

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

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１７−６１０２２号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

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

【0006】

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

【課題を解決するための手段】

【0007】

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

【0008】

位置仮決め部は、第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めする。動作範囲判定部は、位置Ｐ５’及びＰ６’が、ロボットのリンクパラメータに基づく動作範囲内にあるか否かを判定する。逆変換処理部は、位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う。また、順変換処理部は、逆変換処理により求められた各軸の角度を用いて順変換処理を行う。

【0009】

評価値演算部は、制御点の目標位置と、順変換処理により得られた位置との差の位置行列ｐ_Eを求めると共に、目標位置に対応する回転行列に、順変換処理により得られた回転行列の逆行列を乗じて回転行列Ｒ_Eを求める。そして、繰返し演算実行部は、位置行列ｐ_Eのノルムが閾値を超えているか、又は回転行列Ｒ_Eより求められる角度が閾値を超えていると、位置行列ｐ_E及び回転行列Ｒ_Eを反映させた同時変換行列により逆変換処理からの処理を繰り返し実行する。つまり、位置行列ｐ_Eのノルムが閾値以下であり、且つ回転行列Ｒ_Eより求められる角度が閾値以下であれば、逆変換処理が完了する。このように構成すれば、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有するロボットについても、第６軸の角度を仮決めして逆変換処理を行い、その処理結果を順変換処理して得られる行列ｐ_E及び行列Ｒ_Eを評価して演算を収束させることで、逆変換処理を行うことが可能になる。

【0010】

請求項２記載のロボットの制御装置によれば、逆変換処理部は、前記位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように前記位置Ｐ５’及びＰ６’を修正する。これにより、逆変換処理を継続して実行できる。

【0011】

請求項３記載のロボットの制御装置によれば、繰返し演算実行部は、同次変換行列により決まる制御点の位置について、動作範囲判定部と同様に動作範囲内にあるか否かを判断する。そして、制御点の位置が前記動作範囲内になければ、当該動作範囲内に位置するように同次変換行列を修正する。これにより、繰返し演算を継続して実行できる。

【0012】

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

【0013】

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

【図面の簡単な説明】

【0014】

【図1】第１実施形態であり、ロボットシステムの構成を示す図

【図2】ロボットの座標系を示す図

【図3】ロボットの構成を、ｘｚ平面及びｙｚ平面で示す図

【図4】ロボットのアーム形態をｘｙ座標及びαｒ座標で示す図

【図5】ロボットのアーム形態をαｚ座標で示す図

【図6】αｚ座標で定義したアームの各形態を示す図

【図7】コントローラが行う逆変換処理を示すフローチャート

【図8】ｄ２＝ｄ３である場合の動作範囲を説明する図

【図9】同一の手先位置についてロボットアームが取り得る４つの手首形態を説明する図

【発明を実施するための形態】

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

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

【0020】

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

【0021】

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

【0022】

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

【0023】

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

【0024】

【表1】

【0025】

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

【0026】

【数1】

【0027】

【数2】

【0028】

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

【0029】

【数3】

【0030】

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

【0031】

【数4】

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

【数5】

【0038】

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

【0039】

【数6】

【0040】

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

【0041】

【数7】

【0042】

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

【0043】

【数8】

【0044】

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

【数9】

【0045】

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

【0046】

【数10】

【0047】

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

【0048】

【数11】

【0049】

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

【0050】

_{次に角度θ５，}θ_６を求める。

【0051】

【数12】

【0052】

（＊４）式より、角度θ₅が求められる。また、（＊４）式と（＊１），（＊２）式よりｓｉｎθ₆とｃｏｓθ₆とが求められるので、これにより角度θ₆が求められる。

【0053】

【数13】

【0054】

（＊７）,（＊８）式によりｓｉｎθ₄とｃｏｓθ₄とが求められるので、これにより角度θ₄が求められる。尚、（＊４）式に±の複号があるが、これについては、手首形態の指定において４軸角度又は５軸角度の条件を満たす符号を採用する。また、先に角度θ₅が求められているので、（＊８）式をｃｏｓθ₅で除算しても良いし、これらに替えて（＊９）,（＊１０）式を用いても良い。

【0055】

【数14】

【0056】

以上が逆変換処理の概略である。次に、本実施形態の作用について図７及び図８を参照して説明する。図７は、コントローラ３が行う逆変換処理の内容を示すフローチャートである。先ず、６軸の位置を仮決めしてＰ６’とすると、その位置Ｐ６’から５軸の位置を仮決めしてＰ５’とする。そして、これらを用いて同次変換行列Ｔ’を決定する（Ｓ１）。そして、仮決めした位置Ｐ５’が動作範囲内にあるか否かを判断する（Ｓ２）。

【0057】

図８は、前記動作範囲を説明する図であるが、これはｄ２＝ｄ３である場合を前提としている。半径Ｌ１の球Ａと、半径（Ｌ１＋Ｌ２）の球Ｂとを設定する。Ｌ１，Ｌ２は、それぞれ（＊１１）式，（＊１２）式で定義される。

【数15】

【0058】

そして、位置Ｐ５’が球Ａの外にあれば（Ｓ２；動作範囲外）、同次変換行列Ｔ’で決まる位置から動作範囲の境界までの距離をＬとする（Ｓ９）。続いて、距離Ｌがリンク長ｄ５以下か否かを判断する（Ｓ１０）。これは、仮決めした位置Ｐ６’が動作範囲内にあるか否かの判断である。距離Ｌがリンク長ｄ５を超えていれば（ＮＯ）エラーとして処理を終了する。

【0059】

一方、距離Ｌがリンク長ｄ５以下であれば（ＹＥＳ）、位置Ｐ５’を球Ａの内側に移動させるように修正する（Ｓ１４）。例えば、位置Ｐ５’を、位置Ｐ１と位置Ｐ５’とを結ぶ直線と、球Ａとの交点に移動させれば良い。位置Ｐ５’を移動させることに伴い、位置Ｐ６’も移動する。尚、ステップＳ１０では、Ｌ２＝ｄ５として、位置Ｐ５’が球Ｂ外か否かを判断しても良い。

【0060】

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

【0061】

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

【0062】

【数16】

【0063】

ステップＳ２で「動作範囲内」と判断すると、またステップＳ１４を実行すると、逆変換処理を行う（Ｓ３）。ここで、ｇ^-1（Ｔ’，Ｆ）は、ｄ₅＝０として同時変換行列Ｔ’，ロボット２の形態Ｆから角度θ’＝［θ₁’，θ₂’，θ₃’，θ₄’，θ₅’，θ₆’］^Tを求める関数である。
次に、逆変換の結果として求められた角度θ’を用いて順変換処理を行う（Ｓ４）。この順変換処理の結果得られた同時変換行列をＴ”，形態をＦ”とする。同時変換行列Ｔ”は、要素として回転行列Ｒ”，位置行列ｐ”を含む。

【0064】

次に、目標位置ｐと上記位置ｐ”との差を求め、位置誤差ベクトルｐ_Eを求める。また、目標位置ｐに対応する回転行列Ｒに回転行列Ｒ”の逆行列を乗じて評価用の回転行列Ｒ_Eを求める（Ｓ５）。そして、ベクトルｐ_Eのノルム||ｐ_E||が閾値ｐ_EM以下か（Ｓ６）、また、回転行列Ｒ_Eの角度||Ｒ_E||が閾値Ｒ_EM以下かを（Ｓ７）それぞれ判断する。ここで、||ｐ_E||，||Ｒ_E||は、それぞれ（＊１３），（＊１４）式で定義される。

【0065】

【数17】

【0066】

ステップＳ７で「ＹＥＳ」と判断すると、位置ｐ”と回転行列Ｒ”より決まる手首形態が指定した形態か否かを判断し（Ｓ８）、指定した形態が得られていれば逆変換処理を終了する。一方、ステップＳ６又はＳ７で「ＮＯ」と判断すると、逆変換処理及び順変換処理の繰り返し実行回数が予め定めた規定数を超えたか否かを判断する（Ｓ１１）。ここで、規定数を超えると（ＹＥＳ）エラーとして処理を終了する。規定数を超えていなければ（ＮＯ）、回転行列Ｒ’に評価用の回転行列Ｒ_Eを乗じ、位置行列ｐ’に位置誤差ベクトルｐ_Eを加えて同時変換行列Ｔ’を更新する（Ｓ１２）。それから、ステップＳ２と同様に動作範囲内外の判定を行い（Ｓ１３）、動作範囲内であればステップＳ３に移行し、動作範囲外であればステップＳ１４に移行する。

【0067】

次に、ステップＳ８の処理判定について説明する。図９に示すように、本実施形態のように第３オフセットアーム８を有することでｄ５≠０であるロボットの場合、１つの手先位置に対して最大で４つの手首形態があり、ステップＳ９における評価を満たす６軸角度も最大で４つ存在する。そこで、ステップＳ８において、逆変換処理で得られた手首形態を一意に決定するため、ヤコビ行列Ｊを用いる。上記の最大で４つ存在する６軸角度はこれらの境界を分ける点は特異点となるはずであり、ヤコビ行列Ｊは、特異点において行列式がゼロとなる。

【0068】

ヤコビ行列の符号と、従来の手首形態の考え方，４軸角度が−９０度〜９０度，−１８０度〜−９０度又は９０度〜１８０度の何れになるかの組み合わせで、最大で４つ存在する手首形態FLIP+,FLIP-,NONFLIP+,NONFLIP-を判別することができる。

【0069】

すなわち、腕形態RIGHTY,LEFTYと肘形態ABOVE,BELOWに加えて、手首形態FLIP+,FLIP-,NONFLIP+,NONFLIP-の組み合わせがある。そこで、ステップＳ８ではヤコビ行列Ｊの行列式を計算する。ヤコビ行列Ｊは、（８７）式〜（９９）式で表される。

【0070】

【数18】

【0071】

【数19】

【0072】

【数20】

【0073】

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

【0074】

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

【0075】

また、コントローラ３は、第６軸の位置を仮決めし、その仮決めした位置Ｐ６’に基づいて第５軸の位置をＰ５’に仮決めすると、位置Ｐ５’及びＰ６’が、ロボット２のリンクパラメータに基づく動作範囲内にあるか否かを判定する。位置Ｐ５’及びＰ６’が動作範囲内にあれば、それらの位置に基づく同次変換行列について、リンク長ｄ５を「０」に設定して逆変換処理を行う。それから、逆変換処理により求められた各軸の角度を用いて順変換処理を行う。

【0076】

更に、制御点の目標位置と、順変換処理により得られた位置との差の位置誤差ベクトルｐ_Eを求めると共に、目標位置に対応する回転行列に、順変換処理により得られた回転行列の逆行列を乗じて回転行列Ｒ_Eを求める。そして、位置誤差ベクトルｐ_Eのノルムが閾値を超えているか、又は回転行列Ｒ_Eより求められる角度が閾値を超えていると、位置誤差ベクトルｐ_E及び回転行列Ｒ_Eを反映させた同時変換行列によりが逆変換処理からの処理を繰り返し実行する。位置誤差ベクトルｐ_Eのノルムが閾値以下で、且つ回転行列Ｒ_Eより求められる角度が閾値以下であれば、逆変換処理が完了する。

【0077】

このように構成すれば、第４軸と第６軸とを連結するリンク長ｄ５のオフセットアームを有するロボット２についても、第６軸の角度を仮決めして逆変換処理を行い、その処理結果を順変換処理して得られる行列ｐ_E及び行列Ｒ_Eを評価して演算を収束させることで、逆変換処理を行うことが可能になる。

【0078】

また、コントローラ３は、位置Ｐ５’及びＰ６’が動作範囲内になければ、当該動作範囲内に位置するように位置Ｐ５’及びＰ６’を修正する。同次変換行列Ｔ’により決まる制御点の位置についても同様に、動作範囲内にあるか否かを判断し、動作範囲内になければ動作範囲内に位置するように同次変換行列Ｔ’を修正する。これにより、逆変換処理や繰返し演算を継続して実行できる。
そして、コントローラ３は、逆変換処理の結果についてヤコビ行列Ｊの行列式を演算する。そして、行列式の符号が予め指定された手首形態に合致するか否かにより、逆変換処理結果の手首形態を一意に決定できる。

【0079】

本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
コントローラ３を、ロボット２のベース４の外に配置しても良い。

【符号の説明】

【0080】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】