特開 2022-136507 逆運動学演算装置及び逆運動学演算方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022136507

(43)【公開日】2022-09-21

(54)【発明の名称】逆運動学演算装置及び逆運動学演算方法

(51)【国際特許分類】

   B25J 9/06 20060101AFI20220913BHJP

【ＦＩ】

B25J9/06 A

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2021036149

(22)【出願日】2021-03-08

(71)【出願人】

【識別番号】000006666

【氏名又は名称】アズビル株式会社

(74)【代理人】

【識別番号】110003166

【氏名又は名称】弁理士法人山王内外特許事務所

(72)【発明者】

【氏名】茂木 貴弘

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707BS12

3C707BT01

3C707LS16

3C707LT01

3C707LV01

3C707LV14

3C707MS27

(57)【要約】

【課題】Ｗｒｉｓｔ特異点状態であっても、協働作業者にとって安全な逆運動学演算の解を計算可能とする。
【解決手段】Ｗｒｉｓｔ特異点状態である場合、Ｊ６の位置、Ｊ５とＪ６との間のリンク長さ（Ｌ５）、及び、位置制御機構に伴うＪ４の可到達領域に基づいて、多関節ロボットアームと協働作業者との間の相対位置関係から、Ｊ４の位置を計画する第四関節位置計画器１０７と、多関節ロボットアームの先端の姿勢、Ｊ６の位置、及び、第四関節位置計画器１０７により計画されたＪ４の位置から、Ｊ６の角度を計算する第六関節角度計算器１０８とを備えた。
【選択図】図１

【特許請求の範囲】

【請求項1】

根元側から手首第一関節、手首第二関節及び手首第三関節を有するオフセット手首機構、並びに、前記オフセット手首機構の根元に接続された位置制御機構を備えた多関節ロボットアームにおいて、前記手首第二関節の回転動作によって前記手首第一関節の回転軸と前記手首第三関節の回転軸とが平行且つ非同軸となるＷｒｉｓｔ特異点状態である場合、前記手首第三関節の位置、前記手首第二関節と前記手首第三関節との間のリンク長さ、及び、前記位置制御機構に伴う前記手首第一関節の可到達領域に基づいて、前記多関節ロボットアームと協働作業者との間の相対位置関係から、前記手首第一関節の位置を計画する手首第一関節位置計画器と、
前記多関節ロボットアームの先端の姿勢、前記手首第三関節の位置、及び、前記手首第一関節位置計画器により計画された前記手首第一関節の位置から、前記手首第三関節の角度を計算する手首第三関節角度計算器と
を備えた逆運動学演算装置。

【請求項2】

前記位置制御機構は、前記手首第一関節の位置に対する逆運動学演算の解が有限個となる
ことを特徴とする請求項１記載の逆運動学演算装置。

【請求項3】

Ｗｒｉｓｔ特異点状態であるか否かを検出するＷｒｉｓｔ特異点検出器を備え、
前記手首第一関節位置計画器は、前記Ｗｒｉｓｔ特異点検出器によりＷｒｉｓｔ特異点状態であることが検出された場合に、前記手首第一関節の位置を計画する
ことを特徴とする請求項１又は請求項２記載の逆運動学演算装置。

【請求項4】

前記Ｗｒｉｓｔ特異点検出器によりＷｒｉｓｔ特異点ではないことが検出された場合、前記多関節ロボットアームの先端の姿勢から、前記手首第三関節の角度を計算する第２の手首第三関節角度計算器と、
前記手首第三関節の位置及び前記第２の手首第三関節角度計算器により計算された前記手首第三関節の角度から、前記手首第一関節の位置を計算する手首第一関節位置計算器とを備えた
ことを特徴とする請求項３記載の逆運動学演算装置。

【請求項5】

手首第一関節位置計画器が、根元側から手首第一関節、手首第二関節及び手首第三関節を有するオフセット手首機構、並びに、前記オフセット手首機構の根元に接続された位置制御機構を備えた多関節ロボットアームにおいて、前記手首第二関節の回転動作によって前記手首第一関節の回転軸と前記手首第三関節の回転軸とが平行且つ非同軸となるＷｒｉｓｔ特異点状態である場合、前記手首第三関節の位置、前記手首第二関節と前記手首第三関節との間のリンク長さ、及び、前記位置制御機構に伴う前記手首第一関節の可到達領域に基づいて、前記多関節ロボットアームと協働作業者との間の相対位置関係から、前記手首第一関節の位置を計画するステップと、
手首第三関節角度計算器が、前記多関節ロボットアームの先端の姿勢、前記手首第三関節の位置、及び、前記手首第一関節位置計画器により計画された前記手首第一関節の位置から、前記手首第三関節の角度を計算するステップと
を有する逆運動学演算方法。

【発明の詳細な説明】

【技術分野】

【0001】

この発明は、オフセット手首機構を有する多関節ロボットアームに対して逆運動学演算を行う逆運動学演算装置及び逆運動学演算方法に関する。

【背景技術】

【0002】

６つの回転関節（関節）から成る垂直多関節の産業用ロボットアーム（多関節ロボットアーム）として、図７に示すような多関節ロボットアームが多く用いられている。
図７に示すように、多関節ロボットアームは、主に姿勢制御機構及び位置制御機構から構成されると考えることができる。

【0003】

姿勢制御機構は、手首機構とも呼ばれる。ここでは、姿勢制御機構が有する関節を、根元側（位置制御機構側）から、手首第一関節、手首第二関節及び手首第三関節と呼ぶ。

【0004】

また、図７に示す姿勢制御機構は、特に球状直交手首機構と呼ばれている。この球状直交手首機構は、３つの手首関節の回転軸が１点で直交する特徴があり、位置制御機構に対して独立に姿勢制御できることから、非常に多く利用されている。

【0005】

球状直交手首機構は、図７に示すように、手首第二関節の角度によっては、手首第一関節の回転軸と手首第三関節の回転軸とが平行且つ同軸となるので、手首第一関節の角度と手首第三関節の角度との無数の組み合わせで同じ姿勢を表現できてしまう。すなわち、この場合、多関節ロボットアームの逆運動学演算の解は無数であり、特異点である。特異点では、無数の逆運動学演算の解から解を１つ選択する必要があるので、逆運動学演算装置は特異点か否かを認識し、必要に応じて処理を切替えた方がよい。そのため、多関節ロボットアームが特異点状態となり易い機構であるか否かは、逆運動学演算装置にとって重要な観点である。もちろん、位置制御機構由来で特異点が生じることもあるが、位置制御機構由来の特異点については本願発明との関連が薄いため、説明は省略する。

【0006】

一方、近年では、人協働ロボットとして、球状直交手首機構ではない、オフセット手首機構を備えた多関節ロボットアームも多く用いられている。オフセット手首機構は、手首第二関節が回転しても、手首第一関節の回転軸と手首第三関節の回転軸とが平行にはなっても同軸にはならないという構造的特徴がある。オフセット手首機構は、手首第一関節の回転軸と手首第三関節の回転軸とがずれているため、姿勢制御と位置制御が相互に影響してしまい、逆運動学演算の解法が球状直交手首機構よりも複雑となる。オフセット手首機構を備えた多関節ロボットアームの例を図８に示す。

【0007】

図８に示すオフセット手首機構を備えた多関節ロボットアームは、特異点が手首機構由来であるか姿勢制御機構由来であるかを判別可能であり、姿勢制御機構由来の特異点（以下、Ｗｒｉｓｔ特異点と呼ぶ）を手首第二関節の角度だけで検出できるという利点がある。以下、図８に示す多関節ロボットアームを、オフセット手首機構を有する垂直６回転関節ロボットアームと呼ぶ。また、ここでは、垂直６回転関節ロボットアームが有する各関節を、根元から順にＪ１，Ｊ２，・・・，Ｊ６と呼ぶ。また、各関節の角度を、根元から順にθ１，θ２，・・・，θ６と呼ぶ。なお、図８に示すオフセット手首機構は、計算が単純となるように、Ｊ５の回転軸が、Ｊ４の回転軸及びＪ６の回転軸に対して直交としているが、線形独立であれば下記議論は同様に成り立つので、必ずしも直交でなくてもよい。

【0008】

非特許文献１に、オフセット手首機構を有する垂直６回転関節ロボットアームの逆運動学を解析的に解く方法が示されている。非特許文献１に開示された逆運動学演算装置の構成を以下に示す。
逆運動学演算装置は、図９に示すように、入力器３０１、第一関節角度計算器３０２、第一関節座標系変換器３０３、第五関節角度計算器３０４、第六関節角度計算器３０５、第六関節位置計算器３０６、第四関節位置計算器３０７、第二三関節角度計算器３０８、第三関節位置計算器３０９、第四関節角度計算器３１０及び出力器３１１を備えている。

【0009】

入力器３０１は、ユーザからの入力情報を受付ける。入力情報としては、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢、並びに、解選択のための情報が必要である。解選択のための情報とは、関節の角度の初期値でもよいし、解を特定する構造フラグでもよい。また、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢は、垂直６回転関節ロボットアームの可動範囲内の位置及び姿勢である。

【0010】

第一関節角度計算器３０２は、入力器３０１により受付けられた入力情報に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１の角度（θ１）を計算する。ここで、θ１は２通りの値を取り得るため、第一関節角度計算器３０２は、解選択のための情報から一方の値を選択する。

【0011】

第一関節座標系変換器３０３は、入力器３０１により受付けられた入力情報及び第一関節角度計算器３０２による計算結果に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢を、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する。この際、第一関節座標系変換器３０３は、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に、ベース座標系のＺ軸を回転軸として－θ１だけ回転する同次変換行列を掛けることで、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する。

【0012】

第五関節角度計算器３０４は、入力器３０１により受付けられた入力情報及び第一関節座標系変換器３０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置のＹ座標から、Ｊ５の角度（θ５）を計算する。ここで、θ５は２通りの値を取り得るため、第五関節角度計算器３０４は、解選択のための情報から一方の値を選択する。

【0013】

第六関節角度計算器３０５は、第一関節座標系変換器３０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢から、Ｊ６の角度（θ６）を計算する。

【0014】

第六関節位置計算器３０６は、第一関節座標系変換器３０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１座標系におけるＪ６の位置を計算する。

【0015】

第四関節位置計算器３０７は、第六関節角度計算器３０５による計算結果及び第六関節位置計算器３０６による計算結果に基づいて、Ｊ１座標系におけるＪ６の位置及びＪ６の角度（θ６）から、Ｊ１座標系におけるＪ４の位置を計算する。

【0016】

第二三関節角度計算器３０８は、入力器３０１により受付けられた入力情報及び第四関節位置計算器３０７による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ４の位置から、Ｊ２の角度（θ２）及びＪ３の角度（θ３）を計算する。ここで、Ｊ１座標系におけるＪ１の位置は、｛０，０，Ｌ１｝で固定値として既知である。また、θ３は２通りの値を取り得るため、第二三関節角度計算器３０８は、解選択のための情報から一方の値を選択する。

【0017】

第三関節位置計算器３０９は、第二三関節角度計算器３０８による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ２の角度（θ２）から、Ｊ１座標系におけるＪ３の位置を計算する。

【0018】

第四関節角度計算器３１０は、第六関節位置計算器３０６による計算結果、第四関節位置計算器３０７による計算結果及び第三関節位置計算器３０９による計算結果に基づいて、Ｊ１座標系におけるＪ３の位置、Ｊ４の位置及びＪ６の位置から、Ｊ４の角度（θ４）を計算する。

【0019】

出力器３１１は、各関節角度計算器（第一関節角度計算器３０２、第五関節角度計算器３０４、第六関節角度計算器３０５、第二三関節角度計算器３０８及び第四関節角度計算器３１０）により計算された関節の角度をまとめた｛θ１，θ２，θ３，θ４，θ５，θ６｝を示すデータを出力する。

【先行技術文献】

【非特許文献】

【0020】

【非特許文献1】Kelsey P. Hawkins: “Analytic Inverse Kinematics for the Universal Robots UR-5/UR-10 Arms”, 2017

【発明の概要】

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

【0021】

しかしながら、非特許文献１に開示された従来の逆運動学演算装置では、Ｗｒｉｓｔ特異点状態である場合、第六関節角度計算器３０５でθ６を計算で一意に求めることができないと指摘されている。

【0022】

従来の逆運動学演算装置における、Ｗｒｉｓｔ特異点状態である場合の構成を図１０に示す。以下、Ｗｒｉｓｔ特異点状態ではない場合との差異だけ説明する。

【0023】

入力器３０１は、入力情報としてＪ１座標系におけるＪ６の角度（θ６）を更に必要とする。
第四関節位置計算器３０７は、Ｊ１座標系におけるＪ６の位置及び入力情報として与えられたＪ６の角度（θ６）から、Ｊ１座標系におけるＪ４の位置を計算する。

【0024】

このように、θ６を計算で一意に求められないので、ユーザがθ６を適切に決めて、第四関節位置計算器３０７に与える必要があった。しかしながら、ユーザが、θ６の値によってアームがどのように動作するか（どのようなθ２，θ３，θ４が計算されるか）を想像することは非常に困難である。また、θ６の決め方によっては、第四関節位置計算器３０７で到達不能なＪ４の位置が計算され、逆運動学演算の解を得ることさえできない場合もある。
よって、ユーザは試行錯誤（機械による自動的な探索処理を含む）により適切なθ６の値を決める必要があるが、これは、現場作業者の作業効率を低下させる一因となる。

【0025】

また、オフセット手首機構を有する多関節ロボットアームは、人協働ロボットとして多く利用されている構造である故（多関節ロボットアームと人が柵で隔てられず、同じスペースで作業するため）、多関節ロボットアームがどのように動くかわからないというのは多関節ロボットアームと人が接触するリスクを高め、労働安全上望ましくない。

【0026】

以上より、作業効率向上と労働安全性向上のために、Ｗｒｉｓｔ特異点状態であっても、周囲の協働作業者にとって安全な逆運動学演算の解を、試行錯誤することなく計算する方法が求められている。

【0027】

この発明は、上記のような課題を解決するためになされたもので、Ｗｒｉｓｔ特異点状態であっても、協働作業者にとって安全な逆運動学演算の解を計算可能な逆運動学演算装置を提供することを目的としている。

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

【0028】

この発明に係る逆運動学演算装置は、根元側から手首第一関節、手首第二関節及び手首第三関節を有するオフセット手首機構、並びに、オフセット手首機構の根元に接続された位置制御機構を備えた多関節ロボットアームにおいて、手首第二関節の回転動作によって手首第一関節の回転軸と手首第三関節の回転軸とが平行且つ非同軸となるＷｒｉｓｔ特異点状態である場合、手首第三関節の位置、手首第二関節と手首第三関節との間のリンク長さ、及び、位置制御機構に伴う手首第一関節の可到達領域に基づいて、多関節ロボットアームと協働作業者との間の相対位置関係から、手首第一関節の位置を計画する手首第一関節位置計画器と、多関節ロボットアームの先端の姿勢、手首第三関節の位置、及び、手首第一関節位置計画器により計画された手首第一関節の位置から、手首第三関節の角度を計算する手首第三関節角度計算器とを備えたことを特徴とする。

【発明の効果】

【0029】

この発明によれば、上記のように構成したので、Ｗｒｉｓｔ特異点状態であっても、協働作業者にとって安全な逆運動学演算の解を計算可能となる。

【図面の簡単な説明】

【0030】

【図1】実施の形態１に係る逆運動学演算装置の構成例を示す図である。

【図2】実施の形態１に係る逆運動学演算装置の動作例を示すフローチャートである。

【図3】Ｊ４の可到達領域とθ６によるＪ４の計算値域を示す図である。

【図4】垂直６回転関節ロボットアームが協働作業者へ接近するリスクが小さくなる解におけるＪ４の位置の一例を示す図である。

【図5】垂直６回転関節ロボットアームが協働作業者へ接近するリスクが小さくなる解におけるＪ４の位置の別の一例を示す図である。

【図6】実施の形態３に係る逆運動学演算装置の構成例を示す図である。

【図7】一般的な多関節ロボットアームの構成例を示す図である。

【図8】オフセット手首機構を有する垂直６回転関節ロボットアームの原点姿勢定義（各関節の角度が０［ｒａｄ．］）を示す図である。

【図9】従来の逆運動学演算装置の構成例を示す図である。

【図10】従来の逆運動学演算装置の構成例を示す図である（Ｗｒｉｓｔ特異点状態の場合）。

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

【0031】

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１は実施の形態１に係る逆運動学演算装置の構成例を示す図である。
逆運動学演算装置は、オフセット手首機構を有する多関節ロボットアームに対して逆運動学演算を行う。実施の形態１では、逆運動学演算装置が演算対象とする多関節ロボットアームが垂直６回転関節ロボットアームである場合を例に説明を行う。

【0032】

逆運動学演算装置は、図１に示すように、入力器１０１、第一関節角度計算器１０２、第一関節座標系変換器１０３、第六関節位置計算器１０４、第五関節角度計算器１０５、Ｗｒｉｓｔ特異点検出器１０６、第四関節位置計画器１０７、第六関節角度計算器１０８、第六関節角度計算器（第２の第六関節角度計算器）１０９、第四関節位置計算器１１０、第二三関節角度計算器１１１、第三関節位置計算器１１２、第四関節角度計算器１１３及び出力器１１４を備えている。

【0033】

なお、逆運動学演算装置は、システムＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等により実現される。

【0034】

入力器１０１は、ユーザからの入力情報を受付ける。入力情報としては、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢、並びに、解選択のための情報が必要である。解選択のための情報とは、関節の角度の初期値でもよいし、解を特定する構造フラグでもよい。また、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢は、垂直６回転関節ロボットアームの可動範囲内の位置及び姿勢である。

【0035】

第一関節角度計算器１０２は、入力器１０１により受付けられた入力情報に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１の角度（θ１）を計算する。ここで、θ１は２通りの値を取り得るため、第一関節角度計算器１０２は、解選択のための情報から一方の値を選択する。

【0036】

第一関節座標系変換器１０３は、入力器１０１により受付けられた入力情報及び第一関節角度計算器１０２による計算結果に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢を、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する。この際、第一関節座標系変換器１０３は、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に、Ｚ軸を回転軸として－θ１だけ回転する同次変換行列を掛けることで、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する。

【0037】

第六関節位置計算器１０４は、第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１座標系におけるＪ６の位置を計算する。

【0038】

第五関節角度計算器１０５は、入力器１０１により受付けられた入力情報及び第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置のＹ座標から、Ｊ５の角度（θ５）を計算する。ここで、θ５は２通りの値を取り得るため、第五関節角度計算器１０５は、解選択のための情報から一方の値を選択する。

【0039】

Ｗｒｉｓｔ特異点検出器１０６は、第五関節角度計算器１０５による計算結果に基づいて、Ｊ５の角度（θ５）から、Ｗｒｉｓｔ特異点状態であるか否かを検出する。すなわち、Ｗｒｉｓｔ特異点検出器１０６は、｜ｓｉｎ（θ５）｜＜ε_{ｗｒｉｓｔ}である場合、Ｗｒｉｓｔ特異点状態であるとして検出する。ε_{ｗｒｉｓｔ}は微小値である。

【0040】

第四関節位置計画器１０７は、Ｗｒｉｓｔ特異点検出器１０６によりＷｒｉｓｔ特異点状態であることが検出された場合、第六関節位置計算器１０４による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ６の位置、垂直６回転関節ロボットアームが有する各リンクの長さ、並びに垂直６回転関節ロボットアームと協働作業者との間の相対位置関係から、Ｊ４の位置を計画する。なお、Ｊ１座標系におけるＪ１の位置は、｛０，０，Ｌ１｝で固定値として既知である。また、垂直６回転関節ロボットアームが有する各リンクの長さも既知である。

【0041】

第六関節角度計算器１０８は、第一関節座標系変換器１０３による変換結果、第六関節位置計算器１０４による計算結果及び第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢、Ｊ１座標系におけるＪ６の位置及びＪ１座標系におけるＪ４の位置から、Ｊ６の角度（θ６）を計算する。

【0042】

第六関節角度計算器１０９は、Ｗｒｉｓｔ特異点検出器１０６によりＷｒｉｓｔ特異点状態ではないことが検出された場合、第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢から、Ｊ６の角度（θ６）を計算する。

【0043】

第四関節位置計算器１１０は、第六関節位置計算器１０４による計算結果及び第六関節角度計算器１０９による計算結果に基づいて、Ｊ１座標系におけるＪ６の位置及びＪ６の角度（θ６）から、Ｊ１座標系におけるＪ４の位置を計算する。

【0044】

第二三関節角度計算器１１１は、入力器１０１により受付けられた入力情報、並びに、第四関節位置計算器１１０による計算結果又は第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ４の位置から、Ｊ２の角度（θ２）及びＪ３の角度（θ３）を計算する。また、θ３は２通りの値を取り得るため、第二三関節角度計算器１１１は、解選択のための情報から一方の値を選択する。

【0045】

第三関節位置計算器１１２は、第二三関節角度計算器１１１による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ２の角度（θ２）から、Ｊ１座標系におけるＪ３の位置を計算する。

【0046】

第四関節角度計算器１１３は、第六関節位置計算器１０４による計算結果、第三関節位置計算器１１２による計算結果、並びに、第四関節位置計算器１１０による計算結果又は第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系におけるＪ３の位置、Ｊ４の位置及びＪ６の位置から、Ｊ４の角度（θ４）を計算する。

【0047】

出力器１１４は、各関節角度計算器（第一関節角度計算器１０２、第五関節角度計算器１０５、第二三関節角度計算器１１１、第四関節角度計算器１１３、並びに、第六関節角度計算器１０８又は第六関節角度計算器１０９）により計算された関節の角度をまとめた｛θ１，θ２，θ３，θ４，θ５，θ６｝を示すデータを出力する。

【0048】

次に、図１に示す実施の形態１に係る逆運動学演算装置の動作例について、図２を参照しながら説明する。
ここで、図３に示すように、ユーザが指定したθ６によって計算されるＪ４の位置の値域は、Ｊ１座標系のＸＺ平面において、Ｊ６の位置（のＸＺ平面への射影）を中心とする半径Ｌ５の円周上となる。Ｊ４の可到達領域（Ｊ４が到達可能な領域）は、Ｊ１の位置を中心とする半径Ｌ２＋Ｌ３の円の内部である。そして、θ６によって計算されるＪ４の位置の値域は、必ずしもＪ４の可到達領域ではない。そのため、逆運動学演算が可解となるためには、Ｊ４の位置が可到達領域内となるようなθ６を決めねばならない。また、協働作業者と垂直６回転関節ロボットアームとの距離が近い程、接触する危険性があると考えられる。そのため、協働作業者の安全のためには、垂直６回転関節ロボットアームが協働作業者へなるべく接近しないような逆運動学演算の解を得ることが重要となる。
そこで、実施の形態１に係る逆運動学演算装置では、θ６ではなく、Ｊ４の位置を先に決めることで上記課題を解決する。実施の形態１に係る逆運動学演算装置によれば、Ｊ４の位置は可到達領域内から選択できるので、必ず可解となる。また、垂直６回転関節ロボットアームが協働作業者へ接近するリスクがより小さくなる解を求めるには、垂直６回転関節ロボットアームのベースへより近くなるようなＪ４の位置を求めればよい。

【0049】

図１に示す実施の形態１に係る逆運動学演算装置の動作例は、図２に示すように、まず、入力器１０１は、ユーザからの入力情報を受付ける（ステップＳＴ２０１）。入力情報としては、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢、並びに、解選択のための情報が必要である。解選択のための情報とは、関節の角度の初期値でもよいし、解を特定する構造フラグでもよい。また、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢は、垂直６回転関節ロボットアームの可動範囲内の位置及び姿勢である。

【0050】

次いで、第一関節角度計算器１０２は、入力器１０１により受付けられた入力情報に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１の角度（θ１）を計算する（ステップＳＴ２０２）。ここで、θ１は２通りの値を取り得るため、第一関節角度計算器１０２は、解選択のための情報から一方の値を選択する。

【0051】

次いで、第一関節座標系変換器１０３は、入力器１０１により受付けられた入力情報及び第一関節角度計算器１０２による計算結果に基づいて、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢を、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する（ステップＳＴ２０３）。この際、第一関節座標系変換器１０３は、ベース座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に、ベース座標系のＺ軸を回転軸として－θ１だけ回転する同次変換行列を掛けることで、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢に変換する。

【0052】

次いで、第六関節位置計算器１０４は、第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置及び姿勢から、Ｊ１座標系におけるＪ６の位置を計算する（ステップＳＴ２０４）。

【0053】

次いで、第五関節角度計算器１０５は、入力器１０１により受付けられた入力情報及び第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の位置のＹ座標から、Ｊ５の角度（θ５）を計算する（ステップＳＴ２０５）。ここで、θ５は２通りの値を取り得るため、第五関節角度計算器１０５は、解選択のための情報から一方の値を選択する。

【0054】

次いで、Ｗｒｉｓｔ特異点検出器１０６は、第五関節角度計算器１０５による計算結果に基づいて、Ｊ５の角度（θ５）から、Ｗｒｉｓｔ特異点状態であるか否かを検出する（ステップＳＴ２０６）。すなわち、Ｗｒｉｓｔ特異点検出器１０６は、｜ｓｉｎ（θ５）｜＜ε_{ｗｒｉｓｔ}である場合、Ｗｒｉｓｔ特異点状態であるとして検出する。ε_{ｗｒｉｓｔ}は微小値である。

【0055】

このステップＳＴ２０６において、Ｗｒｉｓｔ特異点検出器１０６がＷｒｉｓｔ特異点状態であることを検出した場合、第四関節位置計画器１０７は、第六関節位置計算器１０４による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ６の位置、垂直６回転関節ロボットアームが有する各リンクの長さ、並びに垂直６回転関節ロボットアームと協働作業者との間の相対位置関係から、Ｊ４の位置を計画する（ステップＳＴ２０７）。なお、Ｊ１座標系におけるＪ１の位置は、｛０，０，Ｌ１｝で固定値として既知である。また、垂直６回転関節ロボットアームが有する各リンクの長さも既知である。

【0056】

ここで、例えば、Ｊ１の位置とＪ４の位置との距離が最小となるＪ４の位置は、垂直６回転関節ロボットアームが協働作業者へ接近するリスクが小さいと考えられる。そこで、第四関節位置計画器１０７は、図４の符号４０１に示す位置を、協働作業者にとって安全サイドのＪ４の位置（Ｐ４＝｛ｘ４，ｚ４｝）として求めてもよい。

【0057】

なお、θ６によって計算されるＪ４の位置の値域を表す円の中心は、Ｊ６の位置（のＸＺ平面への射影）であるＰ６＝｛ｘ６，ｚ６｝であり、第六関節位置計算器１０４により計算される。また、Ｊ１の位置は、Ｐ１＝｛０，Ｌ１｝で既知である。この場合、下式（１）でＪ４の位置であるＰ４は計算できる。

【0058】

次いで、第六関節角度計算器１０８は、第一関節座標系変換器１０３による変換結果、第六関節位置計算器１０４による計算結果及び第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢、Ｊ１座標系におけるＪ６の位置及びＪ１座標系におけるＪ４の位置から、Ｊ６の角度（θ６）を計算する（ステップＳＴ２０８）。

【0059】

ここで、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢の回転行列［ｎ_ｘ，ｎ_ｙ，ｎ_ｚ］は既知であり、下式（２）～（４）が成り立つ。よって、二次の連立方程式によりｓｉｎ（θ６）及びｃｏｓ（θ６）を計算でき、ａｒｃｔａｎを計算することでθ６を求められる。ここで、ｎ_ｘｘがゼロか否かで連立方程式の解法が異なるので注意が必要である。このように、実施の形態１に係る逆運動学演算装置は、協働作業者の安全という観点でＪ４の位置を計算でき、また計算されたＪ４の位置を実現するθ６を試行錯誤及び反復演算することなく可解を一意に導出可能である。

【0060】

一方、ステップＳＴ２０６において、Ｗｒｉｓｔ特異点検出器１０６がＷｒｉｓｔ特異点状態ではないことを検出した場合、第六関節角度計算器１０９は、第一関節座標系変換器１０３による変換結果に基づいて、Ｊ１座標系における垂直６回転関節ロボットアームの先端の姿勢から、Ｊ６の角度（θ６）を計算する（ステップＳＴ２０９）。

【0061】

次いで、第四関節位置計算器１１０は、第六関節角度計算器１０９による計算結果及び第六関節位置計算器１０４による計算結果に基づいて、Ｊ１座標系におけるＪ６の位置及びＪ６の角度（θ６）から、Ｊ１座標系におけるＪ４の位置を計算する（ステップＳＴ２１０）。

【0062】

次いで、第二三関節角度計算器１１１は、入力器１０１により受付けられた入力情報、並びに、第四関節位置計算器１１０による計算結果又は第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ４の位置から、Ｊ２の角度（θ２）及びＪ３の角度（θ３）を計算する（ステップＳＴ２１１）。また、θ３は２通りの値を取り得るため、第二三関節角度計算器１１１は、解選択のための情報から一方の値を選択する。

【0063】

次いで、第三関節位置計算器１１２は、第二三関節角度計算器１１１による計算結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ２の角度（θ２）から、Ｊ１座標系におけるＪ３の位置を計算する（ステップＳＴ２１２）。

【0064】

次いで、第四関節角度計算器１１３は、第六関節位置計算器１０４による計算結果、第三関節位置計算器１１２による計算結果、並びに、第四関節位置計算器１１０による計算結果又は第四関節位置計画器１０７による計画結果に基づいて、Ｊ１座標系におけるＪ３の位置、Ｊ４の位置及びＪ６の位置から、Ｊ４の角度（θ４）を計算する（ステップＳＴ２１３）。

【0065】

次いで、出力器１１４は、各関節角度計算器により計算された関節の角度をまとめた｛θ１，θ２，θ３，θ４，θ５，θ６｝を示すデータを出力する（ステップＳＴ２１４）。

【0066】

以上のように、この実施の形態１によれば、逆運動学演算装置は、Ｗｒｉｓｔ特異点状態である場合、Ｊ６の位置、Ｊ５とＪ６との間のリンク長さ（Ｌ５）、及び、位置制御機構に伴うＪ４の可到達領域に基づいて、垂直６回転関節ロボットアームと協働作業者との間の相対位置関係から、Ｊ４の位置を計画する第四関節位置計画器１０７と、垂直６回転関節ロボットアームの先端の姿勢、Ｊ６の位置、及び、第四関節位置計画器１０７により計画されたＪ４の位置から、Ｊ６の角度を計算する第六関節角度計算器１０８とを備えた。これにより、実施の形態１に係る逆運動学演算装置は、Ｗｒｉｓｔ特異点状態であっても、協働作業者にとって安全な逆運動学演算の解を計算可能となる。

【0067】

実施の形態２．
実施の形態１に係る逆運動学演算装置では、第四関節位置計画器１０７が、Ｊ１の位置とＪ４の位置との距離が最小となるＪ４の位置を求める場合を示した。しかしながら、協働作業者の安全の考え方は垂直６回転関節ロボットアームと協働作業者との相対的な位置関係によりそれぞれであり、例えば図５に示すように、第四関節位置計画器１０７は、Ｊ１座標系におけるＸ座標が最小となるＪ４の位置（図５の符号５０１に示す位置）を求めてもよい。

【0068】

なお、第四関節位置計画器１０７の設計バリエーションは当業者であれば多々考えられる。よって、実施の形態１，２では、第四関節位置計画器１０７によるＪ４の位置の計算について２種類の方法を示したが、これに限らず、Ｊ６の位置、Ｊ６の位置を中心とする半径Ｌ５の円、及び、位置制御機構仕様より与えられるＪ４の位置の可到達領域から、協働作業者にとって安全サイドの（垂直６回転関節ロボットアームと協働作業者の距離が遠くなるような）Ｊ４の位置を求める方法であれば、他の計算方法でもよい。

【0069】

実施の形態３．
実施の形態１，２に係る逆運動学演算装置では、演算対象とする多関節ロボットアームが、オフセット手首機構を有する垂直６回転関節ロボットアームである場合を例に説明を行った。しかしながら、逆運動学演算装置が演算対象とする多関節ロボットアームはオフセット手首機構を有する垂直６回転関節ロボットアームに限らず、他の位置制御機構を有する多関節ロボットアームであってもよい。この場合の逆運動学演算装置の構成例について以下に示す。なお、位置制御機構は、手首第一関節の位置に対する逆運動学演算の解が有限個である。

【0070】

逆運動学演算装置は、図６に示すように、入力器２０１、位置制御機構回転関節角度計算器２０２、位置制御機構回転関節座標系変換器２０３、手首第三関節位置計算器２０４、手首第二関節角度計算器２０５、Ｗｒｉｓｔ特異点検出器２０６、手首第一関節位置計画器２０７、手首第三関節角度計算器２０８、手首第三関節角度計算器（第２の手首第三関節角度計算器）２０９、手首第一関節位置計算器２１０、位置制御機構逆運動学演算器２１１、手首第一関節角度計算器２１２及び出力器２１３を備えている。

【0071】

実施の形態２に係る逆運動学演算装置では、実施の形態１に示した逆運動学演算装置の各構成の説明に対し、「第一関節」を「位置制御機構回転関節」に読み替え、「第四関節」を「手首第一関節」に読み替え、「第五関節」を「手首第二関節」に読み替え、「第六関節」を「手首第三関節」に読み替え、「垂直６回転関節ロボットアーム」を「多関節ロボットアーム」に読み替えればよい。

【0072】

また、実施の形態２に係る逆運動学演算装置では、実施の形態１に係る逆運動学演算装置に対し、第二三関節角度計算器１１１及び第三関節位置計算器１１２が、位置制御機構逆運動学演算器２１１として統合されている。

【0073】

位置制御機構逆運動学演算器２１１は、手首第一関節位置計算器２１０による計算結果又は手首第一関節位置計画器２０７による計画結果に基づいて、Ｊ１座標系におけるＪ１の位置及びＪ４の位置から、Ｊ１座標系におけるＪ３の位置を計算する。

【0074】

例えば、位置制御機構が有する第二関節及び第三関節は、回転関節である必要はなく、直動関節でもよい。すなわち、上記第二関節及び第三関節は、回転関節であるか又は直動関節であるかの組合せについて、合計４通りの組合せが考えられる。
位置制御機構回転関節座標系において、位置制御機構の全関節が常に同一の二次元平面上に配置され、位置制御機構の先端を２自由度で制御可能な構成であれば、上記第二関節及び第三関節についての逆運動学演算は計算可能である。よって、上記第二関節及び第三関節は回転関節及び直動関節の何れでもよい。なお、当業者であれば２リンクアームの逆運動学演算は自明であるため、その説明は省略する。また、実施の形態３に係る逆運動学演算装置が演算対象とする位置制御機構は必ずしも３自由度である必要もないため、位置制御機構回転関節が無い場合でも対応可能である。位置制御機構回転関節が無い場合、逆運動学演算装置は、位置制御機構回転関節の角度を０、すなわち、位置制御機構回転関節座標系とベース座標系とが等しいものとして演算を行えばよい。

【0075】

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組合わせ、或いは各実施の形態の任意の構成要素の変形、若しくは各実施の形態において任意の構成要素の省略が可能である。

【符号の説明】

【0076】

１０１ 入力器
１０２ 第一関節角度計算器
１０３ 第一関節座標系変換器
１０４ 第六関節位置計算器
１０５ 第五関節角度計算器
１０６ Ｗｒｉｓｔ特異点検出器
１０７ 第四関節位置計画器
１０８ 第六関節角度計算器
１０９ 第六関節角度計算器（第２の第六関節角度計算器）
１１０ 第四関節位置計算器
１１１ 第二三関節角度計算器
１１２ 第三関節位置計算器
１１３ 第四関節角度計算器
１１４ 出力器
２０１ 入力器
２０２ 位置制御機構回転関節角度計算器
２０３ 位置制御機構回転関節座標系変換器
２０４ 手首第三関節位置計算器
２０５ 手首第二関節角度計算器
２０６ Ｗｒｉｓｔ特異点検出器
２０７ 手首第一関節位置計画器
２０８ 手首第三関節角度計算器
２０９ 手首第三関節角度計算器（第２の手首第三関節角度計算器）
２１０ 手首第一関節位置計算器
２１１ 位置制御機構逆運動学演算器
２１２ 手首第一関節角度計算器
２１３ 出力器
３０１ 入力器
３０２ 第一関節角度計算器
３０３ 第一関節座標系変換器
３０４ 第五関節角度計算器
３０５ 第六関節角度計算器
３０６ 第六関節位置計算器
３０７ 第四関節位置計算器
３０８ 第二三関節角度計算器
３０９ 第三関節位置計算器
３１０ 第四関節角度計算器
３１１ 出力器

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】