(57)【要約】【課題】手首部にオフセットがあるロボットの逆運動学の演算負荷を低減させる。【解決手段】本発明の一実施形態は、第６軸と第４軸とが所定量オフセットしている６軸の垂直多関節ロボットにおいて、第６軸上に取り付けられるエンドエフェクタの位置および姿勢が与えられたときに各関節の角度を求める方法であって、第６軸と第５軸の交点である第１交点を含み第６軸に直交する平面上において、第１交点を中心として所定量を半径とする円周上の点である注目点を順次決定し、注目点が、第５軸と第４軸の交点であると仮定した場合の、第４軸と第３軸の交点である第２交点を算出し、算出された第２交点から注目点に向かう第１ベクトルと、注目点から第１交点に向かう第２ベクトルとの内積値を算出し、内積値の絶対値が所定の閾値以下となるときの注目点を、第５軸と第４軸の交点であると推定する。【選択図】図３
2019177436.tif 000002
【特許請求の範囲】
【請求項１】
第６関節の第６軸と第５関節の第５軸とが垂直で交差し、前記第５軸と第４関節の第４軸とが垂直で交差し、前記第４軸と第３関節の第３軸とが垂直で交差し、前記第６軸と前記第４軸とが所定量オフセットしている６軸の垂直多関節ロボットにおいて、前記第６軸上に取り付けられるエンドエフェクタの位置および姿勢が与えられたときに、前記垂直多関節ロボットの各関節の角度を求めるロボット制御装置であって、
前記第６軸と前記第５軸の交点である第１交点を含み前記第６軸に直交する平面上において、前記第１交点を中心として前記所定量を半径とする円周上の点である注目点を順次決定する決定部と、
前記注目点が、前記第５軸と前記第４軸の交点であると仮定した場合の、前記第４軸と前記第３軸の交点である第２交点を算出する第１算出部と、
前記第１算出部によって算出された第２交点から前記注目点に向かう第１ベクトルと、前記注目点から前記第１交点に向かう第２ベクトルとの内積値を算出する第２算出部と、
前記内積値の絶対値が所定の閾値以下となるときの前記注目点を、前記第５軸と前記第４軸の交点であると推定する推定部と、
を備えた、ロボット制御装置。
【請求項２】
第６関節の第６軸と第５関節の第５軸とが垂直で交差し、前記第５軸と第４関節の第４軸とが垂直で交差し、前記第４軸と第３関節の第３軸とが垂直で交差し、前記第６軸と前記第４軸とが所定量オフセットしている６軸の垂直多関節ロボットにおいて、前記第６軸上に取り付けられるエンドエフェクタの位置および姿勢が与えられたときに、前記垂直多関節ロボットの各関節の角度を求める方法であって、
前記第６軸と前記第５軸の交点である第１交点を含み前記第６軸に直交する平面上において、前記第１交点を中心として前記所定量を半径とする円周上の点である注目点を順次決定し、
前記注目点が、前記第５軸と前記第４軸の交点であると仮定した場合の、前記第４軸と前記第３軸の交点である第２交点を算出し、
前記第１算出部によって算出された第２交点から前記注目点に向かう第１ベクトルと、前記注目点から前記第１交点に向かう第２ベクトルとの内積値を算出し、
前記内積値の絶対値が所定の閾値以下となるときの前記注目点を、前記第５軸と前記第４軸の交点であると推定する、
ロボットの関節の角度を求める方法。
【請求項３】
第６関節の第６軸と第５関節の第５軸とが垂直で交差し、前記第５軸と第４関節の第４軸とが垂直で交差し、前記第４軸と第３関節の第３軸とが垂直で交差し、前記第６軸と前記第４軸とが所定量オフセットしている６軸の垂直多関節ロボットにおいて、前記第６軸上に取り付けられるエンドエフェクタの位置および姿勢が与えられたときに、前記垂直多関節ロボットの各関節の角度を求めるためのプログラムであって、
コンピュータに、
前記第６軸と前記第５軸の交点である第１交点を含み前記第６軸に直交する平面上において、前記第１交点を中心として前記所定量を半径とする円周上の点である注目点を順次決定する手順、
前記注目点が、前記第５軸と前記第４軸の交点であると仮定した場合の、前記第４軸と前記第３軸の交点である第２交点を算出する手順、
前記第１算出部によって算出された第２交点から前記注目点に向かう第１ベクトルと、前記注目点から前記第１交点に向かう第２ベクトルとの内積値を算出する手順、
前記内積値の絶対値が所定の閾値以下となるときの前記注目点を、前記第５軸と前記第４軸の交点であると推定する手順、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置、ロボットの関節の角度を求める方法、および、プログラムに関する。
【背景技術】
【０００２】
手先の３関節の回転軸が１点で交わる軸構成のロボットとして通称ＰＵＭＡ型ロボットが知られている。例えば、特許文献１の図１には、ＰＵＭＡ型ロボットが図示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１６−１４０９１８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記ＰＵＭＡ型ロボットの逆運動学の解は解析的に求められると考えられているが、手首部にオフセットがある場合には、逆運動学の演算の処理負荷は重く時間が掛かる。そのため、ロボットが円滑に動作するのに適した実時間での演算を行うには高速なプロセッサが必要となる。
【０００５】
そこで、本発明は、手首部にオフセットがあるロボットの逆運動学の演算負荷を低減させることを目的とする。
【課題を解決するための手段】
【０００６】
本願の例示的な第１発明は、第６関節の第６軸と第５関節の第５軸とが垂直で交差し、前記第５軸と第４関節の第４軸とが垂直で交差し、前記第４軸と第３関節の第３軸とが垂直で交差し、前記第６軸と前記第４軸とが所定量オフセットしている６軸の垂直多関節ロボットにおいて、前記第６軸上に取り付けられるエンドエフェクタの位置および姿勢が与えられたときに、前記垂直多関節ロボットの各関節の角度を求めるロボット制御装置であって、前記第６軸と前記第５軸の交点である第１交点を含み前記第６軸に直交する平面上において、前記第１交点を中心として前記所定量を半径とする円周上の点である注目点を順次決定する決定部と、前記注目点が、前記第５軸と前記第４軸の交点であると仮定した場合の、前記第４軸と前記第３軸の交点である第２交点を算出する第１算出部と、前記第１算出部によって算出された第２交点から前記注目点に向かう第１ベクトルと、前記注目点から前記第１交点に向かう第２ベクトルとの内積値を算出する第２算出部と、前記内積値の絶対値が所定の閾値以下となるときの前記注目点を、前記第５軸と前記第４軸の交点であると推定する推定部と、を備えた、ロボット制御装置である。
【発明の効果】
【０００７】
本発明によれば、手首部にオフセットがあるロボットの逆運動学の演算負荷を低減させることができる。
【図面の簡単な説明】
【０００８】
【図１】図１は、ロボット実施形態のロボットシステムの概略構成を示す図である。
【図２】図２は、実施形態のロボットについて第２アームを下げた状態の正面である。
【図３】図３は、実施形態のロボットについて第２アームを上げた状態の正面である。
【図４】図４は、実施形態のロボットのリンク座標系を示す図である。
【図５】図５は、モデル化した本実施形態のロボットがある姿勢をとる場合の斜視図である。
【図６】図６は、モデル化した本実施形態のロボットが別の姿勢をとる場合の斜視図である。
【図７】図７は、モデル化した参照ロボットがある姿勢をとる場合の斜視図である。
【図８】図８は、モデル化した参照ロボットが別の姿勢をとる場合の斜視図である。
【図９】図９は、図８の参照ロボットについて関節の回転軸の関係を示す図である。
【図１０】図１０は、図８の参照ロボットの逆運動学の解のアルゴリズムを説明する図である。
【図１１】図１１は、図６の本実施形態のロボットについて関節の回転軸の関係を示す図である。
【図１２】図１２は、本実施形態のロボットの逆運動学の解のアルゴリズムについて説明する図である。
【図１３】図１３は、本実施形態のロボットシステムの構成を示すブロック図である。
【図１４】図１４は、本実施形態のロボット制御装置の機能ブロック図である。
【図１５】図１５は、本実施形態のロボット制御装置によって実行されるフローチャートである。
【発明を実施するための形態】
【０００９】
以下、本発明の実施形態に係るロボット制御装置を含むロボットシステムについて説明する。
なお、以下の説明において、特記しない限り、点はワールド座標系（ロボット座標系）における点を意味する。
【００１０】
（１）ロボットシステム１の構成
先ず、第１の実施形態のロボットシステム１の構成について、図１〜３を参照して説明する。図１は、本実施形態のロボットシステム１の概略構成を示す図である。図２は、本実施形態のロボットについて第２アーム（後述する）を下げた状態の正面である。図３は、本実施形態のロボットについて第２アームを上げた状態の正面である。
【００１１】
本実施形態のロボットＲは、所定の製品の組立や製造等に用いられる多関節ロボットであり、例えば、組立ラインや製造ラインに設置されて使用される。
図２および図３に示すように、ロボットＲは、複数の関節部と複数のアームとを備えている。本実施形態の例では、ロボットＲは、６個の関節部１１〜１６と、第１アーム２１および第２アーム２２と、エンドエフェクタ取付部１８とを備えている。いずれも図示しないが、各関節部には、モータと、モータに連結される減速機と、モータの回転位置を検出するための位置検出機構と、モータおよび位置検出機構が電気的に接続される回路基板とを備えている。
【００１２】
また、ロボットＲは、ロボットＲの基端部分を構成する支持部材Ｂを備えている。支持部材Ｂは、第１関節部１１に相対回動可能に連結されている。第１アーム２１および第２アーム２２は、細長い長手形状に形成されている。具体的には、第１アーム２１および第２アーム２２は、それぞれ細長い円筒状に形成されている。
【００１３】
ロボットＲでは、以下のような固定あるいは連結関係となっている。
(i) 第１関節部１１と第２関節部１２とが相対回動可能に連結されている。
(ii) 第２関節部１２と第１アーム２１の第１端２１１とが固定されている。
(iii) 第１アーム２１の第２端２１２と第３関節部１３とが固定されている。
(iv) 第３関節部１３と第４関節部１４とが相対回動可能に連結されている。
(v) 第４関節部１４と第２アーム２２の第１端２２１とが相対回動可能に連結されている。
(vi) 第２アーム２２の第２端２２２と第５関節部１５とが固定されている。
(vii) 第５関節部１５と第６関節部１６とが相対回動可能に連結されている。
(viii) 第６関節部１６とエンドエフェクタ取付部１８が相対回動可能に連結されている。
【００１４】
エンドエフェクタ取付部１８には、ハンドや工具等のエンドエフェクタを取り付けることが可能となっている。
【００１５】
（２）本実施形態のロボットＲのリンクパラメータと順運動学
以下、本実施形態のロボットＲの機構を説明するために、本実施形態のロボットＲのリンクパラメータとしてＤＨパラメータを示すとともに、参考として本実施形態のロボットＲの順運動学について、図４を参照して説明する。図４は、実施形態のロボットＲのリンク座標系を示す図である。
図２，図３に示したロボットＲにおいて、各関節部によって設定される回転軸同士の交点（Ｚ軸同士の交点）を交点Ｏ１〜Ｏ５、エンドエフェクタの取付位置を点Ｏ６としたときに、点Ｏ１〜Ｏ６を原点とするリンク座標系であるLink1〜Link6座標系が図４に示される。図４では、Link1〜Link6座標系をそれぞれΣＬ１〜ΣＬ６と表記している。なお、図４で示されるリンクは、ロボットＲの図３の姿勢に対応している。
図４に示すように、本実施形態のロボットＲでは、交点Ｏ４と交点Ｏ５の間にリンク間距離ｄ５のオフセットがあることが特徴となっている。
ロボットＲのＤＨ（Denavit-Hartenberg）パラメータは、以下の表１に示すとおりである。なお、図４においてリンク間距離ｄ２，ｄ３に関してｄ２＝ｄ３が成立するため、リンク間距離ｄ２，ｄ３をＤＨパラメータ上はゼロとして差し支えない。
【００１６】
【表１】
2019177436.tif 000003
【００１７】
上記ＤＨパラメータにより、ワールド座標径からLink1座標系(ΣＬ１)に対する同次変換行列Ｔ１は、以下に示すようにしてもとめられる。
【００１８】
【数１】
2019177436.tif 000004
【００１９】
以下、Ｓｎ＝sin(θｎ)、Ｃｎ＝cos(θｎ)としたときに、各リンク座標系間の同次変換行列Ｔ１，Ｔ２，Ｔ３，Ｔ４，Ｔ５，Ｔ６は、以下のとおりとなる。
【００２０】
【数２】
2019177436.tif 000005
【００２１】
以上から、上記各リンク座標系間の同次変換行列からエンドエフェクタの同次変換行列Ｔeeは、以下の式（１）から算出される。式（１）を適用することで、各関節の回転角度がわかればエンドエフェクタの位置および姿勢を得ることができる。
【００２２】
【数３】
2019177436.tif 000006
【００２３】
（３）本実施形態のロボットＲの逆運動学の解の算出アルゴリズム
次に、本実施形態のロボットＲの逆運動学の解の算出アルゴリズム（以下、単に「本実施形態のアルゴリズム」という。）について図５〜１２を参照して説明する。
本実施形態のアルゴリズムは、参照ロボットの逆運動学の解の算出アルゴリズムに基づいているため、以下では、本実施形態のロボットＲの説明に加えて、参照ロボットについても説明する。参照ロボットとは、本実施形態のロボットＲとは異なり、手首のオフセットがないロボット（通称ＰＵＭＡ型ロボット）、すなわち、図４のリンク間距離ｄ５がゼロであるロボットのことである。
【００２４】
（３−１）ロボットのモデル化
先ず、本実施形態のロボットＲを図５および図６に示すようにモデル化して考える。
図５および図６はそれぞれ、本実施形態のロボットＲの関節およびリンクに着目してモデル化したロボットモデルの斜視図であり、それぞれロボットＲの異なる姿勢を表している。図５の本実施形態のロボットモデルの姿勢は、図２に示したロボットＲに近い姿勢となっている。
図５および図６において、本実施形態のロボットモデルは、６個の関節Ｊ１〜Ｊ６を備え、６個の関節Ｊ１〜Ｊ６の軸（回転軸）がそれぞれ軸Ａｘ１〜Ａｘ６である。図２および図３に示した実際のロボットＲでは、軸Ａｘ１〜Ａｘ６の回りを回転する関節Ｊ１〜Ｊ６が、それぞれ関節部１１〜１６に対応している。
軸Ａｘ３と軸Ａｘ４の交点が交点Ｏ３であり、軸Ａｘ４と軸Ａｘ５の交点が交点Ｏ４であり、軸Ａｘ５と軸Ａｘ６の交点が交点Ｏ５である。点Ｏ６は、軸Ａｘ６上のエンドエフェクタの取付位置である。
図５に示すように、本実施形態のロボットモデルでは、軸Ａｘ４と軸Ａｘ６にオフセットｄ５が設けられている。
【００２５】
他方、図７および図８は、それぞれ、参照ロボットの関節およびリンクに着目してモデル化したロボットモデル（以下、「参照ロボットモデル」という。）の斜視図であり、それぞれロボットＲの異なる姿勢を表している。図７は図５と同様の姿勢であり、図８は図６と同様の姿勢である。
図７および図８において、参照ロボットモデルは、６個の関節Ｊ１〜Ｊ６を備え、６個の関節Ｊ１〜Ｊ６の軸がそれぞれ軸Ａｘ１〜Ａｘ６である点は、本実施形態のロボットモデルと同じである。しかし、参照ロボットモデルでは、本実施形態のロボットモデルと異なり、軸Ａｘ４と軸Ａｘ６にオフセットがなく、交点Ｏ４と交点Ｏ５が一致する。
【００２６】
（３−２）参照ロボットモデルの逆運動学の解の算出アルゴリズム
次に、参照ロボットモデルの逆運動学の解の算出アルゴリズムについて、図９および図１０を参照して説明する。図９は、図８の参照ロボットモデルについて関節の回転軸の関係を示す図である。図１０は、図８の参照ロボットモデルの逆運動学の解のアルゴリズムを説明する図である。
図９は、図８に示した参照ロボットモデルの各関節の軸と交点との関係を示している。なお、交点Ｏ１は、軸Ａｘ１と軸Ａｘ２の交点である。
【００２７】
ＰＵＭＡ型ロボットのモデルである参照ロボットモデルは、以下の特徴がある。
参照ロボットモデルでは、関節Ｊ１，Ｊ２，Ｊ３，Ｊ５，Ｊ６のそれぞれの回転角度θ１，θ２，θ３，θ５，θ６の角度にかかわらず、関節Ｊ４が回転しない限り、交点Ｏ１，Ｏ３，Ｏ４，Ｏ５，および点Ｏ６は同一平面上にある。この同一平面を、以下では「アーム平面」という。つまり、関節Ｊ１が回転することでアーム平面自体は関節Ｊ１の回りを回転するが、関節Ｊ４が回転しない限り、交点Ｏ１，Ｏ３，Ｏ４，Ｏ５，および点Ｏ６は常にアーム平面上にある。関節Ｊ４の回転角度θ４を変化させると、エンドエフェクタの取付位置Ｏ６はアーム平面を離れる。
【００２８】
参照ロボットモデルでは、関節Ｊ４の回転角度θ４が変化してエンドエフェクタがアーム平面を離れたとしても、エンドエフェクタの位置および姿勢が与えられれば、アーム平面上にある軸Ａｘ４，軸Ａｘ５，および軸Ａｘ６の交点である交点Ｏ４の位置を計算することができる。すなわち、エンドエフェクタの取付位置の点Ｏ６から軸Ａｘ６に沿ってリンク間距離ｄ６だけ移動させた点が、交点Ｏ４と交点Ｏ５の共通の点になるため、交点Ｏ４の位置を容易に計算することができる。
【００２９】
交点Ｏ４の位置を算出した後は、アーム平面上の幾何学から、関節Ｊ１〜Ｊ３の回転角度を以下のようにして算出することができる。
図１０は、アーム平面における交点Ｏ４，Ｏ３，Ｏ１の位置関係を示した図である。図１０において、点Ｐ４は、交点Ｏ４からワールド座標系のｘｙ平面上に下ろした垂線に対して、交点Ｏ１から引いた垂線の交点である。
先ず図９を参照すると、算出された交点Ｏ４のワールド座標系のｘ，ｙ座標であるＯ４.ｘおよびＯ４.ｙに基づいて、関節Ｊ１の回転角度θ１を以下の式（２）に従って算出できる。
【００３０】
【数４】
2019177436.tif 000007
【００３１】
また、図１０に示すアーム平面において、交点Ｏ１と点Ｐ４の距離Ｌｘと、交点Ｏ１と交点Ｏ４の距離Ｌｙとは、交点Ｏ４のワールド座標系のｚ座標であるＯ４.ｚを用いて、それぞれ以下の式（３），（４）に従って算出できる。
【００３２】
【数５】
2019177436.tif 000008
【００３３】
【数６】
2019177436.tif 000009
【００３４】
よって、図１０から幾何学的に角度θａ，θｂ，θｃを算出できるため、関節Ｊ２，Ｊ３の回転角度θ２，θ３を算出することができる。
【００３５】
（３−３）本実施形態のアルゴリズム
次に、本実施形態のアルゴリズムについて、図１１および図１２を参照して説明する。図１１は、図６の本実施形態のロボットＲについて関節の回転軸の関係を示す図である。図１２は、本実施形態のロボットＲの逆運動学の解のアルゴリズムについて説明する図である。
【００３６】
本実施形態のロボットモデルでは、軸Ａｘ４と軸Ａｘ６がオフセットしているため、交点Ｏ４と交点Ｏ５が一致しない。エンドエフェクタの位置、姿勢が与えられた場合、リンク間距離ｄ６は既知であるため、点Ｏ６から交点Ｏ５は得られるが、交点Ｏ５から交点Ｏ４を得ることができない。これは、軸Ａｘ５がどの方向を向いているかわからないためである。
そこで、交点Ｏ４の位置を推定しながら収束演算を行う。交点Ｏ４は、軸Ａｘ５をｚ軸（図１２においてｚ５）とするリンク座標系のｘｙ平面上において原点を中心とした半径ｄ５の円（当該円を図１１および図１２では、円Ｃ５で示す。）の円周上に位置すると考えられる。
【００３７】
図１２に示すように、円Ｃ５の円周上の点Ｏ４ｔを注目点として決定し、仮に注目点Ｏ４ｔを交点Ｏ４とした場合の交点Ｏ３である候補点Ｏ３ｔを、参照ロボットモデルのアルゴリズムと同様の方法で求める。本実施形態のロボットモデルにおいても参照ロボットモデルと同様に、交点Ｏ１，Ｏ３，Ｏ４によってアーム平面が定まるため、図１０と同様の幾何学的関係に従って候補点Ｏ３ｔを求めることができる。
【００３８】
ロボットＲのリンクの拘束条件から、軸Ａｘ４と軸Ａｘ５は直交するため、候補点Ｏ３ｔから注目点Ｏ４ｔに向かうベクトル（第１ベクトルの例）と、注目点Ｏ４ｔから交点Ｏ５に向かうベクトル（第２ベクトルの例）との内積値がゼロである場合に、注目点Ｏ４ｔが交点Ｏ４であると判断することができる。そこで、円Ｃ５の円周上で注目点Ｏ４ｔを順次移動させ、上記内積値がゼロになる点を探索するように処理を行う。なお、演算時間を考慮して、収束条件として上記内積値の絶対値が所定の閾値以下である場合に、注目点Ｏ４ｔが交点Ｏ４であると推定することが好ましい。
交点Ｏ４の座標が特定された後は、参照ロボットモデルと同様のアルゴリズムによって、関節Ｊ１〜Ｊ３の回転角度θ１〜θ３を算出することができる。
【００３９】
（４）ロボット制御装置３の構成
次に、本実施形態のロボット制御装置３の構成について、図１３を参照して説明する。図１３は、本実施形態のロボットシステムの構成を示すブロック図である。
図１３に示すように、ロボット制御装置３は、制御部３１と、ストレージ３２と、通信部３３とを備える。
【００４０】
制御部３１は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、および、ＲＡＭ（Random Access Memory）を含む。ＣＰＵは、図示しない外部の情報処理装置からロボットプログラムを受信してストレージ３２に記憶するとともに、当該ロボットプログラムをＲＡＭに展開して実行する。制御部３１のＣＰＵがロボットプログラムを実行することで、後述する各機能が実現される。当該ロボットプログラムには、上述した本実施形態のアルゴリズムが組み込まれている。
制御部３１は、ロボットプログラムを実行することで、関節部１１〜１６の各々を動作させるための制御パルスを生成する。
【００４１】
ストレージ３２は、ＨＤＤ（Hard Disk Drive）あるいはＳＳＤ（Solid State Drive）等の大容量記憶装置である。ストレージ３２には、ロボットプログラム、上述したＤＨパラメータ（あるいはリンクパラメータ）、および、ロボットプログラムの実行記録である実行ログデータが格納される。
通信部３３は、他の通信機器と通信を行ってロボットプログラムを受信する通信インタフェース回路を含む。通信部３３は、例えばオペレータが操作するロボット教示装置（図示せず）と通信可能に接続され、当該ロボット教示装置からロボットプログラムを受信する。
【００４２】
図１３に示すように、ロボットＲは、関節部１１〜１６の各々に内蔵されたモータ駆動回路１０１およびモータ１０２を含む。モータ駆動回路１０１は、制御装置３からの指令に基づいてモータ１０２を駆動する。
【００４３】
（５）ロボット制御装置３の機能
次に、図１４を参照して、ロボット制御装置３の制御部３１がロボットプログラムを実行して実現される機能について説明する。図１４は、本実施形態のロボット制御装置３の機能ブロック図である。
図１４に示すように、ロボット制御装置３によって実現される機能には、注目点決定部３１１、第１算出部３１２、第２算出部３１３、および、推定部３１４がある。
【００４４】
既に述べたように、本実施形態のロボットＲは、関節Ｊ６の軸Ａｘ６と関節Ｊ５の軸Ａｘ５とが垂直で交差し、軸Ａｘ５と関節Ｊ４の軸Ａｘ４とが垂直で交差し、軸Ａｘ４と関節Ｊ３の軸Ａｘ３とが垂直で交差し、軸Ａｘ６と軸Ａｘ４とが所定量オフセットしている６軸の垂直多関節ロボットである。本実施形態のロボット制御装置３の制御部３１は、上記ロボットプログラムを実行することで、軸Ａｘ６にエンドエフェクタの位置および姿勢が与えられたときにロボットＲの各関節の角度を求める。
【００４５】
注目点決定部３１１は、軸Ａｘ６と軸Ａｘ５の交点である交点Ｏ５（第１交点の例）を含み軸Ａｘ６に直交する平面上において、交点Ｏ５を中心としてリンク間距離ｄ５（所定量の例）を半径とする円周上の点である注目点Ｏ４ｔを順次決定する機能を備える。「交点Ｏ５を含み軸Ａｘ６に直交する平面」とは、軸Ａｘ５をｚ軸とするリンク座標系のｘｙ平面を意味する。制御部３１は、注目点Ｏ４ｔとして、例えば、図１１および図１２に示した円Ｃ５の円周上を同一の方向に所定の間隔（ピッチ）で逐次移動させた点を決定していく。
【００４６】
第１算出部３１２は、注目点決定部３１１によって決定された注目点Ｏ４ｔが、軸Ａｘ５と軸Ａｘ４の交点Ｏ４であると仮定した場合の、軸Ａｘ４と軸Ａｘ３の交点である候補点Ｏ３ｔ（第２交点の例）を算出する機能を備える。図１０に示したように、アーム平面上の幾何学的関係に基づいて、交点Ｏ４を注目点Ｏ４ｔとした場合の交点Ｏ３である候補点Ｏ３ｔがもとめられる。
【００４７】
第２算出部３１３は、第１算出部３１２によって算出された候補点Ｏ３ｔから注目点Ｏ４ｔに向かう第１ベクトルと、注目点Ｏ４ｔから交点Ｏ５に向かう第２ベクトルとの内積値を算出する機能を備える。注目点Ｏ４ｔ、候補点Ｏ３ｔ、および、交点Ｏ５のロボット座標系での座標が既知であるため、内積値を算出することができる。
【００４８】
推定部３１４は、第２算出部３１３によって算出された内積値の絶対値が所定の閾値以下となるときの注目点Ｏ４ｔを、軸Ａｘ５と軸Ａｘ４の交点Ｏ４であると推定する機能を備える。
【００４９】
（６）ロボット制御装置３の処理
次に、図１５のフローチャートを参照して、本実施形態のアルゴリズムを実装したロボット制御装置３の処理について説明する。
ロボットプログラムには、ロボットの教示情報からエンドエフェクタの位置および姿勢の要求値が記述されている。ロボット制御装置３の制御部３１は、エンドエフェクタの位置および姿勢の要求値から、軸Ａｘ６と軸Ａｘ５の交点Ｏ５を算出する（ステップＳ１０）。エンドエフェクタの取付位置Ｏ６、軸Ａｘ６の方向、および、リンク間距離ｄ６が既知であるため、制御部３１は、交点Ｏ５の座標を算出することができる。
【００５０】
次いで制御部３１は、交点Ｏ５（第１交点の例）を含み軸Ａｘ６に直交する平面上において、交点Ｏ５を中心としてリンク間距離ｄ５を半径とする円Ｃ５の円周上の点である注目点Ｏ４ｔを決定する（ステップＳ１２）。円Ｃ５の円周上の注目点Ｏ４ｔの設定開始位置は、任意に決めてもよい。
注目点Ｏ４ｔが決定されると制御部３１は、図１０に示すアーム平面上の幾何学的関係に基づいて、交点Ｏ３の候補点Ｏ３ｔを算出する（ステップＳ１４）。
【００５１】
次いで制御部３１は、候補点Ｏ３ｔから注目点Ｏ４ｔに向かうベクトルと、注目点Ｏ４ｔから交点Ｏ５に向かうベクトルとの内積値を算出する（ステップＳ１６）。制御部３１は、得られた内積値の絶対値が所定の閾値以下でない限り（ステップＳ１８：ＮＯ）、注目点Ｏ４ｔを円Ｃ５の円周上で所定の間隔（ピッチ）で移動させ（つまり、注目点Ｏ４ｔを変更して）、再度ステップＳ１２〜Ｓ１８の処理を実行する。
【００５２】
ステップＳ１６で得られる内積値が所定の閾値以下となった場合（ステップＳ１８：ＹＥＳ）、候補点Ｏ３ｔと注目点Ｏ４ｔを結ぶ線と、注目点Ｏ４ｔと交点Ｏ５とを結ぶ線が概ね直交し、リンクの拘束条件を満たしたと判断することができる。そこで、制御部３１は、内積値が所定の閾値以下となったときの候補点Ｏ３ｔおよび注目点Ｏ４ｔを、それぞれ交点Ｏ３および交点Ｏ４と決定する（ステップＳ２０）。
【００５３】
次いで制御部３１は、ステップＳ２０で決定された交点Ｏ３，Ｏ４を基に、図１０のアーム平面での幾何学的関係に基づいて、関節Ｊ１〜Ｊ３の回転角度θ１〜θ３を算出する（ステップＳ２２）。
【００５４】
以上説明したように、本実施形態のロボット制御装置３によれば、ロボットＲの手首部にオフセット（つまり、図４のリンク間距離ｄ５）がある場合であっても、エンドエフェクタの位置および姿勢が与えられたときに、比較的簡易な演算によって各関節の回転角度を算出することができる。すなわち、ロボットＲの手首部にオフセットがあるロボットの逆運動学の演算負荷を低減させることができる。
【００５５】
以上、本発明のロボット制御装置の実施形態について詳述したが、本発明は上記の実施形態に限定されない。また、上記の実施形態は、本発明の主旨を逸脱しない範囲において、種々の改良や変更が可能である。
【００５６】
上述した説明により、図１４の機能ブロック図に記載された機能をコンピュータに実現させるためのプログラム、および、当該プログラムが記録されたコンピュータ可読記憶媒体（不揮発性の記憶媒体をも含む。）が開示されていることは当業者に理解される。
【符号の説明】
【００５７】
１…ロボットシステム、３…ロボット制御装置、３１…制御部、３１１…注目点決定部、３１２…第１算出部、３１３…第２算出部、３１４…推定部、３２…ストレージ、３３…通信部、ＷＣ…ケーブル、Ｒ…ロボット、１１〜１６…関節部、１８…エンドエフェクタ取付部、２１，２２…アーム、２１１，２２１…第１端、２１２，２２２…第２端、１０１…モータ駆動回路、１０２…モータ
【図１】
2019177436.tif 000010
【図２】
2019177436.tif 000011
【図３】
2019177436.tif 000012
【図４】
2019177436.tif 000013
【図５】
2019177436.tif 000014
【図６】
2019177436.tif 000015
【図７】
2019177436.tif 000016
【図８】
2019177436.tif 000017
【図９】
2019177436.tif 000018
【図１０】
2019177436.tif 000019
【図１１】
2019177436.tif 000020
【図１２】
2019177436.tif 000021
【図１３】
2019177436.tif 000022
【図１４】
2019177436.tif 000023
【図１５】
2019177436.tif 000024