JP2021122894A - 逆運動学演算装置及び逆運動学演算方法 - Google Patents

Abstract

【課題】従来構成に対し、ロボットアームの手先の位置精度及び姿勢精度を保証可能とする。【解決手段】複数の関節を有するロボットアームの手先の位置及び姿勢、並びに、位置精度及び姿勢精度を入力情報として受付ける入力器１と、入力器１により受付けられた入力情報に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行う数値計算器２と、入力器１により受付けられた入力情報及び数値計算器２により更新された中間変数に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解を求める関節角計算器３と、関節角計算器３により求められた解に対して順運動学演算を行い、入力器１により受付けられた入力情報を満たすかを判定する検算器４と、検算器４による判定結果に基づいて、数値計算器２による更新を停止させる収束判定器５と、検算器４により入力情報を満たすと判定された解を示すデータを出力する出力器６とを備えた。【選択図】図１

Description

この発明は、複数の関節を有するロボットアームに対して逆運動学演算を行う逆運動学演算装置及び逆運動学演算方法に関する。

複数の関節を有するロボットアーム（多関節ロボットアーム）の状態を表現する方法としては、関節空間表現及びデカルト空間表現の２通りの方法がある。

関節空間表現は、ロボットアームが有する各関節の角度（回転角度）によって、ロボットアームの状態を表現する。６軸のロボットアームの場合には、６次元の関節角ベクトルで、ロボットアームの状態を表現できる。なお以下では、垂直多関節ロボットアームの関節を根元側から順にＪ１，Ｊ２，・・・で表し、関節の角度を根元側から順にθ_１，θ_２，・・・で表す。

デカルト空間表現は、デカルト空間におけるロボットアームの手先の位置及び姿勢によって、ロボットアームの状態を表現する。デカルト空間表現では、３次元の位置及び３次元の姿勢の合計６次元のデータで、ロボットアームの状態を表現できる。なお以下では、デカルト空間表現を、Ｐ＝｛ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ｝で表す。

そして、関節空間からデカルト空間へ変換するための演算を順運動学演算と呼び、デカルト空間から関節空間へ変換するための演算を逆運動学演算と呼ぶ。ユーザは、ロボットアームに何らかの作業をさせる際に、デカルト空間表現でロボットアームに対して移動指示を出すことが多いため、逆運動学演算が必要となる。

逆運動学演算は、ロボットアームの手先の位置及び姿勢から、全ての関節又は一部の関節の角度を求める演算である。そのため、逆運動学演算では、愚直にアプローチすると高次元の数値計算を複数回実行する必要があり、計算コストが高くなる。大前提として、逆運動学演算の計算時間は、ロボットアームの制御にとっては遅延となるため、できるだけ低計算コストであることが望ましい。そこで、ロボットアームの構造（幾何特性）を考慮して次元削減し、逆運動学演算の計算コストを低減する研究がなされている。

産業用の垂直６関節ロボットアーム（産業用ロボット）では、図７に示すようなリンク機構が用いられることが多い（例えば非特許文献１参照）。リンク機構は、関節が同一直線上に配置された構成であるため、数値計算を必要とせず、解析的に逆運動学演算を解くことができる。

次に、人協働ロボットアームのリンク機構の一例を図８に示す。
図８において、ロボットアームのＪ４から先端に至る部分（図８に示す破線より上部分）は、手先の姿勢制御を主に担当する手首機構と呼ばれる。手首機構としては、直交球状手首機構、非直交球状手首機構又は非球状手首機構（オフセット手首機構）等様々な機構が提案されている（例えば非特許文献２参照）。
このロボットアームは、ユーザがロボットアームに直接触れてダイレクト教示を行う用途がある。そのため、ダイレクト教示中にユーザの手がロボットアームに挟み込まれないように、手首機構はオフセット手首機構で設計されることが多い。

そして、例えば図８に示すように、Ｊ４の回転軸とＪ６の回転軸とがずれているオフセット手首機構を有するロボットアーム（以下、Ｊ４−６オフセットアームと略記）に対しては、解析的に逆運動学演算する手法は発明されていないため、数値的に演算する必要がある。図８に示すように、Ｊ４−６オフセットアームは、ロボットアームが有する関節のうちの６つの関節において、Ｊ１の回転軸とＪ４の回転軸とＪ６の回転軸とが平行（略平行の意味を含む）であり、且つ、Ｊ４の回転軸とＪ６の回転軸とがオフセットしている。

次に、Ｊ４−６オフセットアームの逆運動学演算の方法について説明する。
ここで、Ｊ４−６オフセットアームのリンク機構と運動学的に等価なリンク機構を図９に示す。図９のように簡略されたリンク機構を用いると運動学の演算はし易くなる。
図９において、点Ｏは、ロボットアームの根元であり、ベース座標系の原点である。点Ｐは、ロボットアームの先端（手先）であり、逆運動学演算の入力情報として点Ｐにおける位置及び姿勢が入力される。点Ｇは、点Ｃをベース座標系のｘｙ平面へ射影した点である。点Ｊは、点Ｇのベース座標系のｚ座標に対してｌ_１を加算した点である。ｌ_１は、点Ｏと点Ｅとの間の長さを表す。ｌ_２は、点Ｅと点Ｄとの間の長さを表す。ｌ_３は、点Ｄと点Ｃとの間の長さを表す。ｌ_４は、点Ｃと点Ｂとの間の長さを表す。ｌ_５は、点Ｂと点Ａとの間の長さを表す。ｌ_６は、点Ａと点Ｐとの間の長さを表す。

非特許文献２では、Ｊ４−６オフセットアームにおいて、図９に示すベクトルＢＣとベクトルＢＡとが直交する幾何特性に着目して逆運動学演算を行う方法が提案されている。
この方法では、ベクトルＢＣとベクトルＢＡとの内積を評価関数とする。なお、評価関数をｆで表す。そして、まず、ｆ（θ_６）＝０となるθ_６を数値的に探索する。そして、ｆ（θ_６）＝０となるθ_６が求められると、ロボットアームの先端から順にθ_５〜θ_１を求めることができる。この方法は、６次元の変数（関節角ベクトル）の数値計算を、１次元の中間変数であるθ_６の数値計算として解くことができるため、６次元の変数の数値計算よりも低計算コストである。

次に、従来の逆運動学演算装置の構成例について、図１０を参照しながら説明する。
従来の逆運動学演算装置は、図１０に示すように、入力器１０１、数値計算器１０２、収束判定器１０３、関節角計算器１０４、出力器１０５及びε記憶器１０６を備えている。

入力器１０１は、入力情報を受付ける。入力情報としては、ロボットアームの手先の位置及び姿勢（デカルト空間表現、デカルト空間表現へ変数変換可能な極座標系、又は、円筒座標系等）、並びに、解への要求精度（位置精度及び姿勢精度）が必須である。

数値計算器１０２は、入力器１０１により受付けられた入力情報に基づいて、θ_６を数値的に探索しながら更新する。なお、数値計算器１０２は、収束判定器１０３によりｆ（θ_６）が０に収束したと判定されるまで更新を繰返す。ｆ（θ_６）は、θ_６に対する評価関数を表す。

収束判定器１０３は、ｆ（θ_６）が０に収束したかを判定する。具体的には、収束判定器１０３は、｜ｆ（θ_６）｜＜εであると判定した場合に、ｆ（θ_６）が０に収束したと判定する。εは微小値である。そして、収束判定器１０３は、ｆ（θ_６）が０に収束したと判定した場合に、数値計算器１０２に対して更新停止指示を出力して数値計算器１０２によるθ_６の更新を停止させる。

関節角計算器１０４は、数値計算器１０２により探索された｜ｆ（θ_６）｜＜εとなるθ_６に基づいて、θ_１〜θ_５を求めることで、逆運動学演算の解（関節角ベクトル）を求める。

出力器１０５は、関節角計算器１０４により求められた逆運動学演算の解を示すデータを出力する。

ε記憶器１０６は、収束判定器１０３で用いられるεを示すデータを記憶する。

大隅久: "ロボット工学の基礎", 精密工学会誌Vol.73 No.10, pp.1123-1126, 2007. Cuong Trinh, Dimiter Zlatanov, Matteo Zoppi, and Rezia Molfino : "A GEOMETRICAL APPROACH TO THE INVERSE KINEMATICS OF 6R SERIAL ROBOTS WITH OFFSET WRISTS", Proceedings of the ASME 2015 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference, DETC2015-47950, 2015.

図１１に示すように、従来の逆運動学演算装置では、εの大きさによって、θ_６に誤差が生じる。θ_６の誤差をΔθ_６で表す。また、この逆運動学演算装置では、数値計算器１０２により得られたθ_６から芋づる式にθ_１〜θ_５を求めているため、これらにも誤差が生じる。θ_１〜θ_５の誤差をΔθ_１〜Δθ_５で表す。そのため、この逆運動学演算装置では、逆運動学演算の解（Θ＝｛θ_１＋Δθ_１，θ_２＋Δθ_２，θ_３＋Δθ_３，θ_４＋Δθ_４，θ_５＋Δθ_５，θ_６＋Δθ_６｝）を順運動学演算で検算した場合に、ロボットアームの手先の位置及び姿勢にも誤差が生じる。

また、ｆ（θ_６）は、入力情報であるロボットアームの手先の位置及び姿勢に依存する。すなわち、εによるθ_６の誤差は、入力情報であるロボットアームの手先の位置及び姿勢に依存する。よって、ある微小な固定値であるεを用いて逆運動学演算を行った場合に、必ずしもロボットアームの手先の位置及び姿勢が所望の精度になるとは限らない。

一般に、逆運動学演算への要求仕様としては、所定の位置精度及び姿勢精度を満たすこと（逆運動学演算の精度を保証すること）が求められる。すなわち、逆運動学演算の解を順運動学演算で検算した場合に、ロボットアームの手先の位置精度及び姿勢精度と一致することが求められる。

この要求仕様を満たすためには、上述した内容を基にεを逆算的に求める等の工夫が必要である。しかしながら、εの最適値は、ロボットアームの構造だけでなく、ロボットアームの位置姿勢によっても異なるため、解析的に設定することが困難である。非特許文献２では、試験的に数パターンのロボットアームの手先の位置及び姿勢について逆運動学演算を解いたことが開示されるのみであり、誤差解析及びεの決定方法は触れられておらず、要求仕様を満たすための検討の余地が残されている。

このように、数値計算の次元を削減する方法は、計算コストの低減の効果がある反面、位置精度及び姿勢精度の保証に課題が残されており、逆運動学演算の精度を保証するための改善が求められている。

また、εは数値計算の反復演算回数に関係する。εを必要以上に小さい値にすると、反復演算回数が増加して計算コストが高くなる。そのため、εは、計算コストを小さくするため、できるだけ大きくしたいという制約がある。

以上から、所定のロボットアームの手先の位置精度及び姿勢精度を保証する逆運動学演算の方法が求められている。

この発明は、上記のような課題を解決するためになされたもので、従来構成に対し、ロボットアームの手先の位置精度及び姿勢精度を保証可能な逆運動学演算装置を提供することを目的としている。

この発明に係る逆運動学演算装置は、複数の関節を有するロボットアームの手先の位置及び姿勢、並びに、位置精度及び姿勢精度を入力情報として受付ける入力器と、入力器により受付けられた入力情報に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行う数値計算器と、入力器により受付けられた入力情報及び数値計算器により更新された中間変数に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解を求める関節角計算器と、関節角計算器により求められた解に対して順運動学演算を行い、入力器により受付けられた入力情報を満たすかを判定する検算器と、検算器による判定結果に基づいて、数値計算器による更新を停止させる収束判定器と、検算器により入力情報を満たすと判定された解を示すデータを出力する出力器とを備えたことを特徴とする。

この発明によれば、上記のように構成したので、従来構成に対し、ロボットアームの手先の位置精度及び姿勢精度を保証可能となる。

実施の形態１に係る逆運動学演算装置の構成例を示す図である。 実施の形態１に係る逆運動学演算装置の動作例を示すフローチャートである。 図９に示すリンク機構のベース座標系のｘｙ平面を示す図である。 図９に示すリンク機構のΠ座標系のｘｚ平面を示す図である。 評価関数の一例を示す図である。 実施の形態２に係る逆運動学演算装置の構成例を示す図である。 産業用ロボットのリンク機構の構成例を示す図である。 Ｊ４−６オフセットアームのリンク機構の構成例を示す図である。 Ｊ４−６オフセットアームのリンク機構（簡略化）の構成例を示す図である。 従来の逆運動学演算装置の構成例を示す図である。 従来の逆運動学演算装置で生じる誤差を説明するための図である。

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１は実施の形態１に係る逆運動学演算装置の構成例を示す図である。
逆運動学演算装置は、複数の関節を有するロボットアームに対して逆運動学演算を行う。この逆運動学演算装置は、図１に示すように、入力器１、数値計算器２、関節角計算器３、検算器４、収束判定器５及び出力器６を備えている。

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

入力器１は、入力情報を受付ける。入力情報には、ロボットアームの手先の位置及び姿勢（デカルト空間表現、デカルト空間表現へ変数変換可能な極座標系、又は、円筒座標系等）、並びに、解への要求精度（位置精度及び姿勢精度）が含まれる。

数値計算器２は、入力器１により受付けられた入力情報に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行う。この数値計算器２は、収束判定器５により更新停止指示が通知されるまで中間変数の更新を繰返す。以下では、数値計算器２が、中間変数として、１次元の中間変数の探索及び更新を行う場合を示す。

関節角計算器３は、入力器１により受付けられた入力情報及び数値計算器２により更新された中間変数に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解（関節角ベクトル）を求める。この関節角計算器３は、数値計算器２により中間変数が更新される度に処理を繰返す。

なお、関節角計算器３により求められた逆運動学演算の解は、検算器４により入力情報を満たすと判定されるまでは暫定的な解である。

検算器４は、関節角計算器３により求められた逆運動学演算の解に対して順運動学演算を行い、入力器１により受付けられた入力情報を満たすかを判定する。この検算器４は、関節角計算器３により逆運動学演算の解が求められる度に処理を繰返す。

収束判定器５は、検算器４による判定結果に基づいて、数値計算器２による更新を停止させる。具体的には、収束判定器５は、検算器４により入力情報を満たすと判定された場合に、数値計算器２に更新停止指示を通知し、数値計算器２による更新を停止させる。このように、検算器４による検算結果を直接用いて収束判定を行う方法を、デカルト空間収束判定法と呼ぶ。

出力器６は、検算器４により入力情報を満たすと判定された逆運動学演算の解を示すデータを出力する。

次に、図１に示す実施の形態１に係る逆運動学演算装置の動作例について、図２を参照しながら説明する。以下では、ロボットアームがＪ４−６オフセットアームであり、中間変数がθ_６である場合を示す。また、ｆ（θ_６）の計算方法は非特許文献２に開示された方法を参考にする。

ここで、従来の逆運動学演算装置では、θ_６，ｆ（θ_６）という２次元空間において、｜ｆ（θ_６）｜＜εを収束条件として、ｆ（θ_６）＝０となるようにθ_６を探索する数値計算を行っている。そのため、この逆運動学演算装置では、εが大きすぎると所定の位置精度及び姿勢精度を満たせず、εが小さすぎるとオーバースペックで反復演算回数が多くなり計算コストが大きくなる。

そこで、実施の形態１に係る逆運動学演算装置では、検算器４による検算結果に基づいて収束判定を行う。検算器４は、逆運動学演算の暫定的な解を用いて順運動学演算を行って検算値を求め、その検算値が入力情報を満たすかを判定する。この検算器４により、所定の位置精度及び姿勢精度が保証される点は、重要な技術思想である。

また、従来の逆運動学演算装置の課題の要因は、位置精度及び姿勢精度に対するεを解析的に得られない点である。
これに対し、実施の形態１における収束判定器５が用いるデカルト空間収束判定法は、「収束条件にεを使用しない」という方法である。すなわち、評価関数が０となる場合に位置誤差及び姿勢誤差が０になる性質を利用し、検算結果を数値計算の収束判定に用いる。

図１に示す実施の形態１に係る逆運動学演算装置の動作例では、図２に示すように、まず、入力器１は、入力情報を受付ける（ステップＳＴ２０１）。入力情報には、ロボットアームの手先の位置及び姿勢、並びに、解への要求精度（位置精度及び姿勢精度）が含まれる。

次いで、数値計算器２は、入力器１により受付けられた入力情報に基づいて、θ_６の探索及び更新を行う（ステップＳＴ２０２）。この数値計算器２は、収束判定器５により更新停止指示が通知されるまでθ_６の更新を繰返す。以下、数値計算器２による処理の詳細について説明する。

ここで、一般に、逆運動学演算の解は複数存在する。それらの解を区別する手段として構造フラグがある。構造フラグは、幾つかの観点での指標を持つ。Ｊ４−６オフセットアームの場合、Ｓｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標が用いられる。

まず、Ｓｈｏｕｌｄｅｒ指標の定義について説明する。
図９に示すリンク機構において、ベース座標系のｘｙ平面（ｚ＝０）について考える。この場合、図３に示すように、点Ｇは点Ｏを中心とする半径ｌ_３の円周上に存在する。また、点Ｂのｘｙ平面への射影を点Ｂ’とすると、ベクトルＯＧとベクトルＧＢ’は直交する。そのため、点Ｂ’を起点とする直線と、点Ｏを中心とする半径ｌ_３の円との接点が、点Ｇ（図３に示す点Ｇ１及び点Ｇ２）となる。すなわち、接点は２つあるため、これらを区別する指標をＳｈｏｕｌｄｅｒ指標と定義する。Ｓｈｏｕｌｄｅｒ指標は、下式（１），（２）のように、ベクトルＯＢ’とベクトルＯＧとの外積ベクトルのｚ軸要素の正負で上記接点をＲｉｇｈｔｙ又はＬｅｆｔｙとして判別できる。式（１）において、ｘ_{ｓｈｏｕｌｄｅｒ}は上記外積ベクトルのｘ軸要素を表し、ｙ_{ｓｈｏｕｌｄｅｒ}は上記外積ベクトルのｙ軸要素を表し、ｚ_{ｓｈｏｕｌｄｅｒ}は上記外積ベクトルのｚ軸要素を表す。なお、ＲｉｇｈｔｙとＬｅｆｔｙの誤判定を防ぐため、式（２）のように、計算上発生する数値誤差程度の微小値だけ境界マージンを定義し、Ｎｅｕｔｒａｌ領域を設けることが望ましい。式（２）において、ε_{ｓｈｏｕｌｄｅｒ}は微小値を表す。

次に、Ｅｌｂｏｗ指標の定義について説明する。
図４のように、ベクトルＧＢ’をｘ軸とし、ベクトルＧＪをｚ軸とする座標系を、説明を分かりやすくするために、Π座標系と呼ぶ。このΠ座標系のｘｚ平面について考える。この場合、点Ｃは点Ｊを中心とする半径ｌ_２の円周上に存在する。また、点Ｃは点Ｂを中心とする半径ｌ_４の円周上に存在するとも言える。よって、２つの円の交点が点Ｃ（図４に示す点Ｃ１及び点Ｃ２）である。すなわち、交点は２つあるため、これらを区別する指標をＥｌｂｏｗ指標と定義する。Ｅｌｂｏｗ指標は、θ_３の正負で上記交点をＡｂｏｖｅ又はＢｅｌｏｗとして判別できる。ここで、Ｓｈｏｕｌｄｅｒ指標と同様に、微小値だけ境界マージンを定義し、Ｎｅｕｔｒａｌ領域を設けることが望ましい。また、Ｓｈｏｕｌｄｅｒ指標と微小値の単位を揃えるため、Ｅｌｂｏｗ指標の判定は下式（３）のように計算することとする。式（３）において、ε_{ｅｌｂｏｗ}は微小値を表す。

次に、点Ｐにおける位置及び姿勢、並びに、１対のＳｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標が与えられた場合でのｆ（θ_６）の計算方法について説明する。なお、Ｓｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標は入力情報として与えられてもよいし、ロボットアームの現在の状態から導出してもよい。

まず、点Ｐにおける位置は、入力情報として与えられる。よって、ベクトルＯＰは計算可能である。また、点Ｐにおける姿勢は、入力情報として与えられ、３×３の回転行列で表すことができる。回転行列は、Ｒ＝［ｓ，ｎ，ａ］で表され、３つの列ベクトル（単位ベクトル）で姿勢を表す。

この場合、点Ａ及び点Ｂに関して下式（４）〜（６）が成り立つ。

また、点Ｇに関して以下が成り立つ。
すなわち、ベクトルＯＢのｚ軸要素を０に置換することで、ベクトルＯＢをｘｙ平面（ｚ＝０）へ射影したベクトルＯＢ’が得られる。この場合、∠Ｂ’ＯＧに関して下式（７）〜（９）が成り立つ。なお、式（８）におけるｓｉｎの符号は、Ｓｈｏｕｌｄｅｒ指標によって定まる。すなわち、Ｓｈｏｕｌｄｅｒ指標がＲｉｇｈｔｙであればｓｉｎの符号は＋であり、Ｓｈｏｕｌｄｅｒ指標がＬｅｆｔｙであればｓｉｎの符号は−である。

またこの場合、下式（１０）のように、ベクトルＯＧは、ベクトルＯＢ’を∠Ｂ’ＯＧについて回転変換し、長さをｌ_３へ変換することで求められる。

また、点Ｊに関して、ベクトルＯＪは、ベクトルＯＧのｚ軸要素であるｚ_ｊをｌ_１へ置換することで求められる。

また、点Ｃに関して以下が成り立つ。
すなわち、余弦定理により、∠ＢＪＣに関して下式（１１）〜（１３）が成り立つ。

また、点Ｊから見た点Ｂの仰角である∠ＢＪＸは、下式（１４），（１５）から求められる。なお、式（１４）において、ｚ_ＢはベクトルＯＢのｚ軸要素である。ｚ_Ｊは点Ｊのｚ軸要素である。

またこの場合、点Ｊから見た点Ｃの仰角である∠ＣＪＸは、下式（１６）〜（１８）で表され、Ｓｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標によって第２項の符号が定まる。

また、Ｊ１についての回転変換をＴ１とすると、ベース座標系におけるベクトルＪＣは、下式（１９）〜（２３）のように、Π座標系におけるベクトルＪＣ_ΠをＴ１で回転変換することで求められる。式（２２）において、ｘ_Ｇは点Ｇのｘ軸要素である。式（２３）において、ｙ_Ｇは点Ｇのｙ軸要素である。

以上から、ベクトルＯＣは、下式（２４）で表される。

ベクトルＣＢとベクトルＢＡとが直交することが目的であるため、ベクトルＣＢとベクトルＢＡとの内積を評価関数とする。また、内積をそのまま用いると評価関数の値域がベクトル長に依存してしまう。そのため、下式（２５）のように正規化したものを評価関数として用いるのが望ましい。

そして、数値計算器２は、ｆ（θ_６）が０となるθ_６を求める。ここで、ｆ（θ_６）は、未知の非線形関数であり、ｆ（θ_６）＝０となるθ_６の個数も未知である。ｆ（θ_６）の一例を図５に示す。

数値計算器２による処理（根の探索アルゴリズム）では、「ｆ（θ_６）が未知の非線形関数であるとはいえ、ある微小な区間に着目すれば単調変化である」ことに着目している。数値計算器２による処理は２段階構成であり、ｆ（θ_６）＝０となるθ_６を１つだけ含む区間を求める大域探索と、大域探索で求めた区間についてｆ（θ_６）＝０となるθ_６を求める詳細探索とを行う。ここで、区間を［θ_ｓ，θ_ｅ］で表す。θ_ｓは区間の始点を表し、θ_ｅは区間の終点を表す。

まず、数値計算器２による大域探索について説明する。
ここでは、一例として、数値計算器２がグリッドサーチによる大域探索を行う場合を示す。すなわち、数値計算器２は、θ_６の値域をΔθで複数の［θ_ｓ，θ_ｅ］に分割する。ここで、値域を［θ_{ｌｏｗｅｒ}，θ_{ｕｐｐｅｒ}］で表す。θ_{ｌｏｗｅｒ}は値域の始点を表し、θ_{ｕｐｐｅｒ}は値域の終点を表す。なお、｜θ_{ｌｏｗｅｒ}｜，｜θ_{ｕｐｐｅｒ}｜≦πとする。Δθは微小区間幅を表す。また、ｉ番目の区間について、θ_ｓ［ｉ］及びθ_ｅ［ｉ］は下式（２６），（２７）のように表せる。

そして、数値計算器２は、分割した複数の［θ_ｓ，θ_ｅ］に対してゼロ点包含判定を行う。すなわち、数値計算器２は、［θ_ｓ，θ_ｅ］においてｆ（θ_６）が単調変化する場合であって、θ_ｓ，θ_ｅについて｜ｆ（θ_ｓ）｜＜Ｅ又は｜ｆ（θ_ｅ）｜＜Ｅを満たす場合、又は、ｆ（θ_ｓ）とｆ（θ_ｅ）とが異符号である場合に、［θ_ｓ，θ_ｅ］内にｆ（θ_６）＝０となるθ_６を含むと判定する。Ｅは、微小値であるが、収束判定で用いられるεとは別物である。Ｅとしては、ｆ（θ_６）＝０となるθ_６を見逃さないように、大きめの値（例えば１０^−４）が設定されることが望ましい。

次に、数値計算器２による詳細探索について説明する。
数値計算器２は、大域探索で求めた［θ_ｓ，θ_ｅ］について、ｆ（θ_６）＝０となるθ_６を求める。すなわち、数値計算器２は、ｆの根を数値計算で求める。未知の非線形関数の根を数値計算により求めるアルゴリズムとしては、二分法、割線法又は挟み撃ち法等が挙げられる。ここでは、数値計算器２がアンダーソン・ビョーク法（ＡＢ法）を用いた場合について説明する。

ＡＢ法は、挟み撃ち法の改良手法である。ＡＢ法は、挟み撃ち法と同様に、θ_ｓ又はθ_ｅを更新して根を含む区間を狭めて絞込みを行い、根を求める。このＡＢ法は、挟み撃ち法よりも少ない計算回数で根を求めることができる利点がある。

ＡＢ法による更新点の計算方法（更新方法）は、挟み撃ち法と同じ下式（２８）の通りである。式（２８）において、θ_ｔｅｍｐは更新点を表す。

ここで、数値計算器２は、ｆ（θ_ｓ）とｆ（θ_ｔｅｍｐ）とが同符号である場合には［θ_ｓ，θ_ｔｅｍｐ］内に根はないので、θ_ｓをθ_ｔｅｍｐで更新する。この場合、ＡＢ法では係数であるｍを用い、次回のθ_ｔｅｍｐの計算時にｆ（θ_ｅ）をｍｆ（θ_ｅ）に置換して計算する。ｍは下式（２９）で定義される。計算の結果、ｍが負となってしまった場合には、ｍ＝０．５とする。

なお、ｍ＝０．５で固定値計算する手法は、イリノイ法と呼ばれる。収束の速さは、挟み撃ち法＜イリノイ法≦ＡＢ法の順となる。

一方、数値計算器２は、ｆ（θ_ｓ）とｆ（θ_ｔｅｍｐ）とが異符号である場合には［θ_ｓ，θ_ｔｅｍｐ］内に根があるので、θ_ｅをθ_ｔｅｍｐで更新する。この場合、ＡＢ法ではｍを使い、次回のθ_ｔｅｍｐの計算時にｆ（θ_ｓ）をｍｆ（θ_ｓ）に置換して計算する。ｍは下式（３０）で定義される。計算の結果、ｍが負となってしまった場合は、ｍ＝０．５とする。

次いで、関節角計算器３は、入力器１により受付けられた入力情報及び数値計算器２により更新されたθ_６に基づいて、Θ＝｛θ_１，θ_２，θ_３，θ_４，θ_５，θ_６｝を求める（ステップＳＴ２０３）。この関節角計算器３は、数値計算器２によりθ_６が更新される度に処理を繰返す。

以下、関節角計算器３による処理の詳細（θ_６以外の関節の角度の導出方法）について説明する。なお、各関節のゼロ点と回転方向の定義によって導出式が異なるため、以下では、Ｊ４−６オフセットアームの場合での導出式を示す。

関節角計算器３は、中間変数であるθ_６、並びに、１対のＳｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標が与えられると、他の関節の角度を求められる。なお、関節角計算器３が求める関節の角度の値域は［−π，π］であるとする。関節の角度の値域が［−π，π］より広い場合には、関節の角度は２π周期で一致することに着目して値域を調整すればよい。

まず、関節角計算器３は、下式（３１）により、θ_１を求める。式（３１）におけるｓｉｎθ_１，ｃｏｓθ_１はベクトルＯＧから求められる。この場合、値域が［−π／２，３π／２］であるため、関節角計算器３は、θ_１がπより大きい場合には、θ_１＝θ_１−２πとし、値域が［−π，π］となるように調整する。

また、関節角計算器３は、下式（３２）により、θ_２を求める。この場合、値域が［−π／２，３π／２］であるため、関節角計算器３は、θ_２がπより大きい場合には、θ_２＝θ_２−２πとし、値域が［−π，π］となるように調整する。

また、関節角計算器３は、下式（３３），（３４）により、θ_３を求める。

また、関節角計算器３は、下式（３５）〜（３７）により、θ_４を求める。式（３５）において、Ｙ_ΠはΠ座標系Ｙ軸ベクトルを表し、ベクトルＹ_ΠとベクトルＢＡとがなす角がθ_４である。

なお、関節角計算器３は、θ_４の符号については、下式（３８）から求める。

また、関節角計算器３は、下式（３９），（４０）により、θ_５を求める。すなわち、θ_５は、ベクトルａとベクトルＣＢとがなす角である。

なお、関節角計算器３は、θ_５の符号については、下式（４１）から求める。

以上から、関節角計算器３は、関節角ベクトル（逆運動学演算の解）であるΘが得られる。

なお、関節角計算器３により求められたΘは、検算器４により入力情報を満たすと判定されるまでは、暫定的な値である。

次いで、検算器４は、関節角計算器３により求められたΘに対して順運動学演算を行い、入力情報により受付けられた入力情報を満たすかを判定する（ステップＳＴ２０４）。この検算器４は、関節角計算器３によりΘが求められる度に処理を繰返す。

以下、検算器４による処理の詳細について説明する。
まず、検算器４は、関節角計算器３により求められたΘに基づいて、ロボットアームの手先の位置及び姿勢を求める。ここで、Θに基づくロボットアームの手先の位置及び姿勢をＰ＝Ｆ（Θ）で表す。

そして、検算器４は、入力情報であるロボットアームの手先の位置及び姿勢と、上記Θに基づくロボットアームの手先の位置及び姿勢との誤差を求める。ここで、入力情報であるロボットアームの手先の位置及び姿勢をＰ_{ｔａｒｇｅｔ}で表し、ロボットアームの手先の位置及び姿勢の誤差をΔＰで表す。この場合、ΔＰは下式（４２）〜（４４）で表される。なお式（４２）において、Δｐは位置誤差を表し、Δｑは姿勢誤差を表している。また、式（４４）におけるＱは四元数ベクトルである。四元数ベクトルは、姿勢の表現方法の一種であり、オイラー角ベクトル｛ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ｝より一意に変換できる。姿勢誤差は、四元数ベクトル同士のなす角である。

そして、検算器４は、下式（４５），（４６）を共に満たす場合に、入力情報を満たすと判定する。式（４５）におけるεｐは微小値であり、ユーザにより指定される逆運動学演算の位置精度［ｍ］である。また、式（４６）におけるεｑは微小値であり、ユーザにより指定される逆運動学演算の姿勢精度［ｒａｄ．］である。

ここで、実施の形態１に係る逆運動学演算装置は、ｆ（θ_６）→０となるようθ_６が更新されていくときΔｐ→０且つΔｑ→０となる性質を利用している。そのため、要求精度に対して必要十分な反復演算回数で詳細探索の数値計算を収束させることができる。

次いで、収束判定器５は、検算器４により入力情報を満たすと判定された場合に、数値計算器２に更新停止指示を通知し、数値計算器２による更新を停止させる（ステップＳＴ２０５）。

また、出力器６は、検算器４により入力情報を満たすと判定されたΘを示すデータを出力する（ステップＳＴ２０６）。

以上のように、この実施の形態１によれば、逆運動学演算装置は、複数の関節を有するロボットアームの手先の位置及び姿勢、並びに、位置精度及び姿勢精度を入力情報として受付ける入力器１と、入力器１により受付けられた入力情報に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行う数値計算器２と、入力器１により受付けられた入力情報及び数値計算器２により更新された中間変数に基づいて、ロボットアームの手先の位置及び姿勢についての逆運動学演算の解を求める関節角計算器３と、関節角計算器３により求められた解に対して順運動学演算を行い、入力器１により受付けられた入力情報を満たすかを判定する検算器４と、検算器４による判定結果に基づいて、数値計算器２による更新を停止させる収束判定器５と、検算器４により入力情報を満たすと判定された解を示すデータを出力する出力器６とを備えた。これにより、実施の形態１に係る逆運動学演算装置は、従来構成に対し、ロボットアームの手先の位置精度及び姿勢精度を保証可能となる。

実施の形態２．
実施の形態１では、収束判定器５が、検算器４による検算結果を直接用い、収束判定を行う場合を示した（デカルト空間収束判定法）。これに対し、実施の形態２では、収束判定器５ｂが、検算器４による検算結果を間接的に用い、収束判定を行う場合を示す（最適ε法）。
図６は実施の形態２に係る逆運動学演算装置の構成例を示す図である。この図６に示す実施の形態２に係る逆運動学演算装置は、図１に示す実施の形態１に係る逆運動学演算装置に対し、ε記憶器７及び補償数値計算器８を追加し、収束判定器５を収束判定器５ｂに変更している。その他の構成は同様であり、同一の符号を付してその説明を省略する。

収束判定器５ｂは、検算器４による判定結果に基づいて、数値計算器２による更新を停止させる。具体的には、収束判定器５ｂは、数値計算器２により更新された中間変数に対する評価関数を求め、当該評価関数がε未満であると判定した場合に、数値計算器２に更新停止指示を通知し、数値計算器２による更新を停止させる。

なお、収束判定器５ｂは、数値計算器２で探索及び更新される中間変数が２次元以上である場合には、複数のεを用いて収束判定を行ってもよい。なお、εの次元は、中間変数の次元以下である。

ε記憶器７は、収束判定器５ｂで用いられるεを示すデータを記憶する。ε記憶器７は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）又はメモリ等によって構成される。

図６では、ε記憶器７が逆運動学演算装置の内部に設けられた場合を示している。しかしながら、これに限らず、ε記憶器７は逆運動学演算装置の外部に設けられていてもよい。

補償数値計算器８は、検算器４により入力情報を満たしていないと判定された場合に、εを更新する。すなわち、補償数値計算器８は、要求精度（位置精度及び姿勢精度）を満たすようにεを更新し、ε記憶器７へ記憶させる。εを小さくすると位置誤差及び姿勢誤差は小さくなるので、補償数値計算器８は、例えばε→０．９εというようにεを小さくするように更新を行う。εの初期値としては、ある程度大きめの値（例えば１０^−４）が設定される。

最適ε法は、「最適なεを数値計算により求める」方法であり、従来技術通り、収束条件にεを使用する。一方で、最適ε法では、暫定的なεにより求めた逆運動学演算の暫定的な解を検算器４で検算し、その検算結果が所定の要求精度を満たさない場合には、当該要求精度を満たすようにεを更新し、逆運動学演算の解を再度求める。この最適ε法は、二重に数値計算を行うため、実施の形態１で示したデカルト空間収束判定法より計算コストで劣るが、従来の逆運動学演算装置へ後付けし易く、拡張性の面で有利である。

なお、実施の形態１，２では、中間変数が１次元であり、数値計算器２が１次元の数値計算を行う場合を示したが、これに限らない。すなわち、実施の形態１，２に係る逆運動学演算装置において最も効果的であるのは中間変数が１次元の場合であるが、中間変数の次元はロボットアームが有する関節の次元未満であればよい。

また、実施の形態１，２では、数値計算器２がθ_６（Ｊ６の角度）を中間変数として探索及び更新を行う場合を示した。しかしながら、これに限らず、数値計算器２は、他の関節の角度又は座標値を中間変数として探索及び更新を行ってもよい。

また、実施の形態１，２では、数値計算器２及び関節角計算器３による処理方法として、非特許文献２で開示された処理方法を用いた場合を示した。しかしながら、これに限らず、数値計算器２及び関節角計算器３による処理方法は、低次元の中間変数を用いて上記と同様の演算を行う処理方法であれば他の処理方法でもよい。

また、実施の形態１，２では、ロボットアームとしてＪ４−６オフセットアームを例に説明を行ったが、複数の関節を有するロボットアームであればよい。

また、実施の形態１，２では、逆運動学演算装置が、逆運動学演算の解を１つだけ求める場合を示した。しかしながら、これに限らず、逆運動学演算装置は、複数の逆運動学演算の解を求めてもよい。例えば、逆運動学演算は、Ｓｈｏｕｌｄｅｒ指標及びＥｌｂｏｗ指標の組合わせについて網羅的に演算する、又は、ｆ（θ_６）＝０となる全てのθ_６を探索する等して、複数の逆運動学演算の解を求めてもよい。

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

１ 入力器
２ 数値計算器
３ 関節角計算器
４ 検算器
５，５ｂ 収束判定器
６ 出力器
７ ε記憶器
８ 補償数値計算器

Claims (6)

1. 複数の関節を有するロボットアームの手先の位置及び姿勢、並びに、位置精度及び姿勢精度を入力情報として受付ける入力器と、
   前記入力器により受付けられた入力情報に基づいて、前記ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行う数値計算器と、
   前記入力器により受付けられた入力情報及び前記数値計算器により更新された中間変数に基づいて、前記ロボットアームの手先の位置及び姿勢についての逆運動学演算の解を求める関節角計算器と、
   前記関節角計算器により求められた解に対して順運動学演算を行い、前記入力器により受付けられた入力情報を満たすかを判定する検算器と、
   前記検算器による判定結果に基づいて、前記数値計算器による更新を停止させる収束判定器と、
   前記検算器により入力情報を満たすと判定された解を示すデータを出力する出力器と
   を備えた逆運動学演算装置。
2. 前記数値計算器は、中間変数として、１次元の中間変数の探索及び更新を行う
   ことを特徴とする請求項１記載の逆運動学演算装置。
3. 前記ロボットアームは、６以上の関節を有し、当該関節のうちの６つの関節において、根元側から第１の関節の回転軸と第４の関節の回転軸と第６の関節の回転軸とが平行であり、且つ、当該第４の関節の回転軸と当該第６の関節の回転軸とがオフセットしている垂直多関節ロボットアームであり、
   前記数値計算器は、中間変数として、前記第６の関節の角度の探索及び更新を行う
   ことを特徴とする請求項２記載の逆運動学演算装置。
4. 前記収束判定器は、前記検算器により入力情報を満たすと判定された場合に、前記数値計算器による更新を停止させる
   ことを特徴とする請求項１から請求項３のうちの何れか１項記載の逆運動学演算装置。
5. 前記収束判定器は、前記数値計算器により更新された中間変数に対する評価関数を求め、当該評価関数が微小値未満であると判定した場合に、当該数値計算器による更新を停止させ、
   前記検算器により入力情報を満たしていないと判定された場合に、前記微小値を更新する補償数値計算器を備えた
   ことを特徴とする請求項１から請求項３のうちの何れか１項記載の逆運動学演算装置。
6. 入力器が、複数の関節を有するロボットアームの手先の位置及び姿勢、並びに、位置精度及び姿勢精度を入力情報として受付けるステップと、
   数値計算器が、前記入力器により受付けられた入力情報に基づいて、前記ロボットアームの手先の位置及び姿勢についての逆運動学演算の解に対応する中間変数の探索及び更新を行うステップと、
   関節角計算器が、前記入力器により受付けられた入力情報及び前記数値計算器により更新された中間変数に基づいて、前記ロボットアームの手先の位置及び姿勢についての逆運動学演算の解を求めるステップと、
   検算器が、前記関節角計算器により求められた解に対して順運動学演算を行い、前記入力器により受付けられた入力情報を満たすかを判定するステップと、
   収束判定器が、前記検算器による判定結果に基づいて、前記数値計算器による更新を停止させるステップと、
   出力器が、前記検算器により入力情報を満たすと判定された解を示すデータを出力するステップと
   を有する逆運動学演算方法。

Images