JP4894657B2 - 逆運動学計算方法及び逆運動学計算装置 - Google Patents

Description

本発明は、複数の関節を有するロボット（多関節ロボット）の関節変数を計算する逆運動学計算方法及び逆運動学計算装置に関するものである。

従来から、多関節ロボットの作業部位の位置及び姿勢を指定した上で、その位置及び姿勢を実現するための関節角度を計算する逆運動学計算方法が知られている。例えば、下記特許文献１には、多関節リンク機構の逆運動学解析の効率化を実現するための手法が開示されている。下記特許文献１に記載の逆運動学解析方法は、隣接リンク間の機構学的関係を示す関係式と最終リンクの位置及び姿勢とが与えられた場合に、非線形特性を模擬する制御変数である補正係数をガウス・ニュートン法に導入し、目的関数が単調減少するようにその補正係数を変えつつ反復計算を行うことで関節角度を算出する。
特開平７−１４６７０３号公報 特開平７−３６５１８号公報

しかしながら、上記特許文献１に記載の手法では、補正係数（制御変数）を経験的に決定しているに過ぎないので、ロボットの機構を定義するパラメータを変更した際に所望の計算結果が得られない可能性がある。例えば、算出される関節角度が局所解に陥るなどして、所望の結果とは異なる計算結果に収束することがある。

本発明は、上記課題を解決するためになされたものであり、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出することが可能な逆運動学計算方法及び逆運動学計算装置を提供することを目的とする。

本発明の逆運動学計算方法は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、制御変数を決定する制御変数決定ステップと、制御変数決定ステップにおいて決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップであって、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行する、該関数変数算出ステップとを含み、制御変数決定ステップでは、関節変数を評価する所定の評価指標値を算出するための評価関数を用いて、関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて、該変化量が正から負、又は負から正に切り替わる回数を取得し、取得した該回数に基づいて制御変数を決定する、ことを特徴とする。

また、本発明の逆運動学計算装置は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、制御変数を決定する制御変数決定手段と、制御変数決定手段により決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段であって、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行する、該関節変数算出手段とを備え、制御変数決定手段が、関節変数を評価する所定の評価指標値を算出するための評価関数を用いて、関節変数算出手段により算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて、該変化量が正から負、又は負から正に切り替わる回数を取得し、取得した該回数に基づいて制御変数を決定する、ことを特徴とする。

このような逆運動学計算方法及び逆運動学計算装置によれば、制御変数に基づいて関節変数の変化量が算出され、この変化量に基づいて関節変数が算出される。関節変数の算出において用いられる制御変数は、関節変数の算出を安定化する（関節変数の発散を抑制する）ために用いられるパラメータであり、関節変数を評価する所定の評価指標値の変動状況に基づいて決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数が局所解に陥ることを防止することが可能になる。

この場合、関節変数の算出が反復実行され、その結果得られた複数の関節変数のそれぞれについて評価指標値が算出される。この評価指標値は、所定の評価関数を用いて算出される。そして、算出された複数の評価指標値の変化量に基づいて評価指標値の変動状況が導出され、導出された評価指標値の変動状況に基づいて制御変数が決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数の算出を安定化できる（関節変数の発散を抑制できる）。更に、評価指標値の変動状況は、複数の評価指標値の変化量に基づいて導出されるので、より適切な制御変数を決定できる。

本発明の逆運動学計算方法において、関節変数算出ステップでは、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。

また、本発明の逆運動学計算装置において、関節変数算出手段は、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。

この場合、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量が算出される。まず、ガウス・ニュートン法を利用することで、関節変数ベクトルの算出の安定化を実現できる。また、ガウス・ニュートン法に導入される制御変数は、評価指標値の変動状況に基づいて決定されるので、算出対象の関節変数が局所解に陥ることを防止し、所望の関節変数を算出することが可能になる。

このような逆運動学計算方法及び逆運動学計算装置によれば、関節変数の算出において用いられる制御変数が、該関節変数を評価する所定の評価指標値の変動状況に基づいて決定されるので、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。

以下、添付図面を参照しながら本発明の実施形態を詳細に説明する。本実施の形態では、本発明に係る逆運動学計算装置を、所定の位置に固定された多関節ロボットの関節角度を計算する逆運動学計算装置１に適用する。なお、図面の説明において同一又は同等の要素には同一の符号を付し、重複する説明を省略する。

図１〜３を参照して、本実施の形態に係る逆運動学計算装置１について説明する。図１は、本実施形態に係る逆運動学計算装置１の構成を示す図である。図２は、図１に示す逆運動学計算装置１が適用されるロボットＲを模式的に示す図である。図３（ａ）及び図３（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の例を示すグラフである。

逆運動学計算装置１は、複数の関節を有するロボットの先端（作業部位）の位置及び姿勢（以下「先端位置及び先端姿勢」ともいう）の入力を受け付け、逆運動学計算により、入力された先端位置及び先端姿勢から各関節の関節角度を算出する。特に、逆運動学計算装置１は、制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいて目標の関節変数を算出する。このとき、逆運動学計算装置１は、算出された関節変数を評価する所定の評価指標値の変動状況に基づいて上記制御変数を決定する。

このために逆運動学計算装置１は、入力部２、変化量計算部３、評価部４、収束判定部５、及び出力部６を備えている。逆運動学計算装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、変化量計算部３、評価部４及び収束判定部５は、ハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。なお、本実施形態では、変化量計算部３が特許請求の範囲に記載する関節変数算出手段に相当し、評価部４が特許請求の範囲に記載する制御変数決定手段に相当する。

まず、逆運動学計算装置１が適用されるロボットＲについて説明する。図２に示すように、ロボットＲは、ｎ個の関節Ｊ_１〜Ｊ_ｎと、関節同士を接続するリンクＬ_１〜Ｌ_ｎ＋１と、先端のリンクＬ_ｎ＋１の一端Ｔに取り付けられたハンドＨとを備えている。このロボットＲは、末端のリンクＬ_１の一端がベースＢに取り付けられることで固定されている。各関節Ｊ_１〜Ｊ_ｎにはアクチュエータが内蔵されている。このアクチュエータによって各関節Ｊ_１〜Ｊ_ｎがそれぞれ回転し、隣接する二本のリンク間の角度θ_１〜θ_ｎが変更される。

逆運動学計算装置１は、ロボットＲの先端Ｔの位置及び姿勢（ハンドＨの位置及び姿勢）から各関節Ｊ_１〜Ｊ_ｎの角度θ_１〜θ_ｎを決定する装置である。ロボットＲの位置及び姿勢は、ｎ個の関節変数が作る関節空間からデカルト空間への非線形写像と考えられる。ロボットＲの先端位置及び先端姿勢から各関節の角度を決定するということは、この非線形写像の逆像の中から解を見つけることである。

逆運動学計算装置１の各機能的構成要素について説明する前に、この逆運動学計算装置１が用いる理論について説明する。

関節Ｊ_１〜Ｊ_ｎのそれぞれに対応する関節変数（関節角度を示す関節変数）をそれぞれｑ１〜ｑｎとすると、関節変数ベクトルはｑ＝（ｑ１，ｑ２，…，ｑｎ）^Ｔ、すなわち（ｑ１，ｑ２，…，ｑｎ）の転置行列で表される。このとき、ロボットＲの先端位置（ハンドＨの位置）はＸ（ｑ），Ｙ（ｑ），Ｚ（ｑ）で表され、ロボットＲの先端姿勢（ハンドＨの姿勢）を定義する成分はＰ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）で表される。また、ロボットＲの先端Ｔの目標位置をＸ_ｇ，Ｙ_ｇ，Ｚ_ｇとし、ロボットＲの先端Ｔの目標姿勢を定義する成分をＰ１_ｇ，Ｐ２_ｇ，…Ｐｍ_ｇとする。

ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑ），Ｙ（ｑ），Ｚ（ｑ），Ｐ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）は、同次変換行列^０Ｔ_ｎの各成分座標である。同次変換行列^０Ｔ_ｎとは、ベースＢの座標系Σ_０から見たハンドＨの座標系Σ_ｎを表す行列であり、以下のように表される。なお、式（１）ではｍ＝９であるが、ｍが他の値（例えば２，４，６など）であってもよい。

導出された先端位置及び先端姿勢と目標位置及び目標姿勢との差を以下の式で表す。すなわち、ロボットＲの先端位置とその目標値との差をｆ_１，ｆ_２，ｆ_３とし、ロボットＲの先端姿勢を定義する成分とその目標値との差をｆ_４，ｆ_５，…，ｆ_ｍ＋３で表す。

式（２）で示されるｆ_１，ｆ_２，…，ｆ_ｍ＋３をまとめ、ｆ（ｑ）＝（ｆ_１，ｆ_２，…，ｆ_ｍ＋３）^Ｔを定義する。なお、（ｆ_１，ｆ_２，…，ｆ_ｍ＋３）^Ｔは、（ｆ_１，ｆ_２，…，ｆ_ｍ＋３）の転置行列である。更に、評価指標値Ｆ（ｑ）を算出するための評価関数Ｆ（ｑ）＝ｆ（ｑ）^Ｔ・ｆ（ｑ）を定義する。この評価指標値Ｆ（ｑ）が０になれば、ロボットＲの先端Ｔの目標位置及び目標姿勢を実現する各関節Ｊ_１〜Ｊ_ｎの角度θ_１〜θ_ｎが算出されたことになる。

この評価指標値Ｆ（ｑ）が０になるようなｆ（ｑ）を算出するために、関節変数ベクトルｑとして適当な値ｑ_ｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））^Ｔを与える。続いて、このｑ_ｋを式（２）に適用することでｆ（ｑ_ｋ）を算出し、算出されたｆ（ｑ_ｋ）を評価関数に当てはめることで評価指標値Ｆ（ｑ_ｋ）を算出する。

続いて、ガウス・ニュートン法の理論を利用して、関節変数ベクトルの変化量Δｑ_ｋを導出する。すなわち、下記式（３）を満たすΔｑ_ｋを導出する。式（３）において、Ｊはヤコビ行列（Ｊ＝｛∂ｆ_ｉ／∂ｑ_ｉ｝）であり、Ｉは単位行列である。また、λは補正操作のための制御変数であり、正数値である。なお、関節変数ベクトルｑ_ｋの変化量Δｑ_ｋを算出するということは、関節変数ｑ１〜ｑｎの変化量を算出するということである。
（数３）
（Ｊ^Ｔ・Ｊ＋λＩ）Δｑ_ｋ＝−Ｊ^Ｔ（ｑ_ｋ）ｆ（ｑ_ｋ） …（３）

式（３）において制御変数λを０とすれば、式（３）はガウス・ニュートン法そのものを表す。ガウス・ニュートン法をそのまま利用した場合（制御変数λを導入しない場合）、関節変数ベクトルｑの設定によっては、関節変数ベクトルが発散することがある。また、行列Ｊ^Ｔ・Ｊが正則でない場合には解が無数に存在してしまうなどの問題が発生する。そこで、行列Ｊ^Ｔ・Ｊの対角項に正数値を加える操作を施す。このように制御変数λを導入したガウス・ニュートン法を用いることで、関節変数ベクトルの算出の安定化を図る。

ただし、制御変数λを固定した場合には、関節変数ベクトルが局所解に陥り、先端Ｔの
目標位置及び目標姿勢を実現する関節角度θ_１〜θ_ｎを得られないことがある。そこで、逆運動学計算装置１は、所定の演算を行なうことで制御変数λを自動的に更新する。制御変数λの更新方法については後述する。

変化量Δｑ_ｋが決まると、下記式（４）により新たな関節変数ベクトルｑ_ｋ＋１が算出される。
（数４）
ｑ_ｋ＋１＝ｑ_ｋ＋Δｑ_ｋ …（４）

例えば、関節変数ベクトルの初期値ｑ_０＝（ｑ１（０），ｑ２（０），…，ｑｎ（０））^Ｔを式（２）に代入することでｆ（ｑ_０）が算出され、更に評価指標値Ｆ（ｑ_０）が算出可能となる。また、式（３）により関節変数ベクトルｑ_０に対する変化量Δｑ_０が算出される。続いて、式（４）にｑ_０及びΔｑ_０を代入することで新たな関節変数ベクトルｑ_１＝ｑ_０＋Δｑ_０が算出される。

次に、算出された関節変数ベクトルｑ_１を式（２）に適用することでｆ（ｑ_１）が算出され、これにより評価指標値Ｆ（ｑ_１）が算出可能となる。また、式（３）により関節変数ベクトルｑ_１に対する変化量Δｑ_１が算出され、式（４）により新たな関節変数ベクトルｑ_２が算出される。以後、このような演算を繰り返すことで、Ｆ（ｑ_ｋ）＝０となるような関節変数ベクトルｑ_ｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））^Ｔが算出され、先端Ｔの目標位置及び目標姿勢を実現する関節角度θ_１〜θ_ｎが導出される。

次に、逆運動学計算装置１の各機能的構成要素について説明する。

入力部２は、逆運動学計算に必要な値の入力を受け付ける手段である。値の入力は、オペレータまたはエンコーダによって行われる。入力部２により受け付けられる値は、ベースＢの位置（ロボットＲの立ち位置）、先端Ｔの目標位置Ｘ_ｇ，Ｙ_ｇ，Ｚ_ｇ、先端Ｔの目標姿勢を定義する成分Ｐ１_ｇ，Ｐ２_ｇ，…Ｐｍ_ｇ、関節Ｊ_１〜Ｊ_ｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ_０、及び一回の収束判定までに関節変数ベクトルを繰り返し算出する回数（評価データ数）Ｓである。入力部２は、受け付けた値を変化量計算部３に出力する。

変化量計算部３は、入力部２又は後述する収束判定部５からの入力値を式（２）〜（４）に適用することでＳ個の関節変数ベクトルｑ_ｋ（ｋ＝１〜Ｓ）を導出する手段である。このために変化量計算部３は、式（２）〜（４）が規定されているプログラムを予めメモリ（図示せず）から読み出す。変化量計算部３は、評価データ数Ｓの分だけ式（２）〜（４）の演算を繰り返すことにより、関節変数ベクトルｑ_ｋ（ｋ＝１〜Ｓ）を算出する。変化量計算部３は、導出されたＳ個の関節変数ベクトルｑ_ｋと、式（３）の演算で用いた制御変数λとを評価部４に出力する。

評価部４は、変化量計算部３から入力されたＳ個の関節変数ベクトルｑ_ｋに基づいて式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）^Ｔ・ｆ（ｑ）を計算することでＳ個の評価指標値Ｆ（ｑ_ｋ）（ｋ＝１〜Ｓ）を算出し、これら複数の評価指標値Ｆ（ｑ_ｋ）の変動状況を取得する手段である。このために評価部４は、式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）^Ｔ・ｆ（ｑ）が規定されているプログラムを予めメモリ（図示せず）から読み出す。具体的には、評価部４は、算出したＳ個の評価指標値Ｆ（ｑ_ｋ）（ｋ＝１〜Ｓ）のそれぞれについてＦ（ｑ_ｋ）からＦ（ｑ_ｋ＋１）への変化量ΔＦ_ｋが正の値であるか負の値であるかを検査し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、下記式（５）を用いて制御変数λを更新（決定）する。
（数５）
λ＝λ_０（１＋ｊ／Ｓ） …（５）

このような評価部４の処理を、図３（ａ）及び（ｂ）を用いて具体的に説明する。なお、図３（ａ）及び（ｂ）のグラフにおいて、縦軸は評価指標値Ｆ（ｑ）であり、横軸は関節変数ベクトルである。図３（ａ）及び（ｂ）に示す例では、評価データ数Ｓが５に設定されているものとする。

図３（ａ）において、Ｆ（ｑ_１）からＦ（ｑ_２）への変化量ΔＦ_１は負の値であるが、Ｆ（ｑ_２）からＦ（ｑ_３）への変化量ΔＦ_２は正の値である。すなわち、Ｆ（ｑ_１）からＦ（ｑ_２）にかけては評価指標値Ｆ（ｑ）が減少しているが、Ｆ（ｑ_２）からＦ（ｑ_３）にかけては増加している。同様に、変化量ΔＦ_３は負の値であり、変化量ΔＦ_４は正の値である。したがって、ΔＦ_１からΔＦ_２にかけては変化量が負の値から正の値に切り替わり、ΔＦ_２からΔＦ_３にかけては変化量が正の値から負の値に切り替わり、ΔＦ_３からΔＦ_４にかけては変化量が負の値から正の値に切り替わっている。すなわち、切替り回数ｊは３である。この場合、制御変数λは、λ_０（１＋３／５）＝１．６λ_０となる。

これに対して図３（ｂ）の場合、変化量ΔＦ_ｋ（ｋ＝１〜４）はすべて負の値であるから、切替り回数ｊは０である。この場合、制御変数λは、λ_０（１＋０／５）＝λ_０となる。

すなわち、評価部４は、評価指標値Ｆ（ｑ）が増加と減少とを繰り返すなどしてその変動が大きい場合には制御変数λを大きくし、評価指標値Ｆ（ｑ）が単調減少するなどしてその変動が小さい場合には制御変数λを小さくする。このように制御変数λを更新する理由は、評価指標値Ｆ（ｑ）の変動が大きいほど制御変数λを大きくして関節変数ベクトルの変化量Δｑ_ｋを小さく抑えることで、関節変数ベクトルｑ_ｋの不安定な挙動を回避するためである。

評価部４は、更新された制御変数λと、Ｓ回目に算出された評価指標値Ｆ（ｑ_Ｓ）と、Ｓ−１回目及びＳ回目に算出された関節変数ベクトルｑ_Ｓ，ｑ_Ｓ−１とを収束判定部５に出力する。

収束判定部５は、入力された評価指標値Ｆ（ｑ_Ｓ）が所定の正数値αよりも小さいか、又は関節変数ベクトルの変化量Δｑ_Ｓ−１＝ｑ_Ｓ−ｑ_Ｓ−１が所定の正数値βよりも小さい場合に、ロボットＲの各関節Ｊ_１〜Ｊ_ｎの角度を示す関節変数ベクトルｑ_Ｓ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））^Ｔが目標値に到達したと判定する手段である。言い換えれば、収束判定部５は、ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑ_Ｓ），Ｙ（ｑ_Ｓ），Ｚ（ｑ_Ｓ），Ｐ１（ｑ_Ｓ），Ｐ２（ｑ_Ｓ），…，Ｐｍ（ｑ_Ｓ）がＸ_ｇ，Ｙ_ｇ，Ｚ_ｇ，Ｐ１_ｇ，Ｐ２_ｇ，…Ｐｍ_ｇに到達したと判定する。この場合、収束判定部５は、関節変数ベクトルｑ_Ｓ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））^Ｔを出力部６に出力する。

これに対し、入力された評価指標値Ｆ（ｑ_Ｓ）が正数値α以上であり、且つ入力された関節変数ベクトルの変化量Δｑ_Ｓ−１が正数値β以上である場合、収束判定部５は、関節変数ベクトルｑを再計算すると判定し、入力された更新後の制御変数λと、関節変数ベクトルｑ_Ｓとを変化量計算部３に出力する。

収束判定部５は、このような処理を行うために、所定の正数値α，βを予めメモリ（図示せず）から読み出す。本来、収束判定部５は、正数値αとして０を読み出し、入力された評価指標値Ｆ（ｑ_Ｓ）が０である場合に関節変数ベクトルｑ_Ｓ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））^Ｔが目標値に到達したと判定するのが好ましい。しかし、評価指標値Ｆ（ｑ_Ｓ）が０か否かのみに基づいて関節変数ベクトルｑ_Ｓの収束の有無を判定すると、関節変数ベクトルｑ_Ｓが局所解に陥ってしまい、逆運動学計算装置１による処理がいわゆる無限ループに陥る可能性がある。そこで、収束判定部５は、二種類の正数値α，βを用いて収束判定を行なう。

出力部６は、収束判定部５から入力された関節変数ベクトルｑ_Ｓを、ロボットＲを動作させる制御部（図示せず）に出力する手段である。出力部６から図示しない制御部に関節変数ベクトルｑ_Ｓが出力させることで、ロボットＲの各関節Ｊ_１〜Ｊ_ｎのアクチュエータが作動し、各関節Ｊ_１〜Ｊ_ｎの角度θ_１〜θ_ｎが変更される。その結果、ハンドＨの位置及び姿勢が目標とするものに変更される。

次に、図４を用いて、図１に示す逆運動学計算装置１の処理を説明するとともに本実施形態に係る逆運動学計算方法について説明する。図４は、逆運動学計算装置１の処理を示すフローチャートである。

逆運動学計算装置１では、まず、入力部２が、オペレータ又はエンコーダから逆運動学計算に必要な値の入力を受け付ける（ステップＳ１１）。具体的には、入力部２は、ベースＢの位置、ロボットＲの先端Ｔの目標位置及び目標姿勢、関節Ｊ_１〜Ｊ_ｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ_０、及び評価データ数Ｓの入力を受け付ける。

続いて変化量計算部３が、入力された関節変数の初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、及び制御変数λ_０を用いて、反復計算によりＳ個の関節変数ベクトルｑ_ｋ（ｋ＝１〜Ｓ）を算出する（ステップＳ１２、関節変数算出ステップ）。具体的には、変化量計算部３は、関節変数の初期値に基づいて導出されたロボットＲの先端位置及び先端姿勢と目標とする先端位置及び先端姿勢との差ｆ_１，ｆ_２，…，ｆ_ｍ＋３を式（２）により算出する。続いて変化量計算部３は、算出されたｆ_１，ｆ_２，…，ｆ_ｍ＋３に基づいてｆ（ｑ_ｋ）を定義し、そのｆ（ｑ_ｋ）を制御変数の初期値λ_０を導入したガウス・ニュートン法に当てはめることで関節変数の変化量Δｑ_ｋを導出する。そして、変化量計算部３は、その変化量Δｑ_ｋと関節変数ベクトルｑ_ｋを式（４）に当てはめることで次の関節変数ベクトルｑ_ｋ＋１を導出する。変化量計算部３は、式（２）〜（４）を用いたこのような処理を評価データ数Ｓの分だけ繰り返す。

続いて評価部４が、導出されたＳ個の関節変数ベクトルｑ_ｋ（ｋ＝１〜Ｓ）に基づいてＳ個の評価指標値Ｆ（ｑ_ｋ）（ｋ＝１〜Ｓ）を導出し（ステップＳ１３、制御変数決定ステップ）、評価指標値Ｆ（ｑ_ｋ）の変動状況を取得する（ステップＳ１４、制御変数決定ステップ）。具体的には、評価部４は、Ｓ個の評価指標値Ｆ（ｑ_ｋ）のそれぞれについてＦ（ｑ_ｋ）からＦ（ｑ_ｋ＋１）への変化量ΔＦ_ｋを算出し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、評価データ数Ｓ、制御変数の初期値λ_０、及び導出された切替り回数ｊを式（５）に当てはめることで制御変数λを更新する（ステップＳ１５、制御変数決定ステップ）。言い換えれば、評価部４は、評価指標値Ｆ（ｑ_ｋ）の変動状況に基づいて制御変数を決定する。

続いて収束判定部５が、評価指標値Ｆ（ｑ_Ｓ）が所定の正数値αよりも小さいか又は関節変数ベクトルの変化量Δｑ_Ｓ−１＝ｑ_Ｓ−ｑ_Ｓ−１が所定の正数値βよりも小さいか否かを判定する（ステップＳ１６）。ここでＦ（ｑ_Ｓ）＜α又はΔｑ_Ｓ−１＜βである場合（ステップＳ１６；ＹＥＳ）、収束判定部５は、関節変数ベクトルｑ_Ｓが目標値に到達した、すなわちロボットＲの先端の目標位置及び目標姿勢を実現する各関節Ｊ_１〜Ｊ_ｎの角度θ_１〜θ_ｎが導出されたと判定する。そして、収束判定部５は、関節変数ベクトルｑ_Ｓを出力部６に出力する。その後、出力部６が導出された関節変数ベクトルｑ_ＳをロボットＲを動作させる制御部（図示せず）に出力する（ステップＳ１７）。これにより、各関節Ｊ_１〜Ｊ_ｎのアクチュエータが作動して各関節の角度θ_１〜θ_ｎが変わり、先端Ｔ（ハンドＨ）の位置及び姿勢が目標とするものに変更される。

これに対し、ここでＦ（ｑ_Ｓ）≧α且つΔｑ_Ｓ−１≧βである場合（ステップＳ１６；ＮＯ）、収束判定部５は、入力された更新後の制御変数λと関節変数ベクトルｑ_Ｓとを変化量計算部３に出力する。これにより、今度はｑ_Ｓを初期値として、ステップＳ１２以降の処理が実行される。なお、ステップＳ１２における計算では、更新後の制御変数λを導入したガウス・ニュートン法が用いられる。

以上説明したように、本実施形態によれば、制御変数λが、経験的に決められるのではなく評価指標値Ｆ（ｑ）の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数λを決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数λを評価指標値Ｆ（ｑ）の変動状況に基づいて決定することで、関節変数ベクトルｑ_ｋの算出を安定化する（関節変数ベクトルｑ_ｋの発散を抑制する）とともに、関節変数ベクトルｑ_ｋが局所解に陥ることを防止することが可能になる。

また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況は、評価データ数Ｓの分だけ算出された評価指標値Ｆ（ｑ_ｋ）（ｋ＝１〜Ｓ）の変化量Ｆ_ｋ（ｋ＝１〜Ｓ−１）に基づいて導出されるので、より適切な制御変数を決定できる。

また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況に基づいて決定される制御変数λを導入したガウス・ニュートン法を用いることで（式（３）を用いることで）、算出対象の関節変数ベクトルｑ（関節変数ｑ１〜ｑｎ）が局所解に陥ることを防止し、所望の関節変数ベクトル（関節変数）を算出することが可能になる。

以上、本発明をその実施形態に基づいて詳細に説明した。しかし、本発明は上記実施形態に限定されるものではない。本発明は、その要旨を逸脱しない範囲で以下のような様々な変形が可能である。

本実施形態では、本発明に係る逆運動学計算装置を、ベースＢに固定され多数の関節を有するロボットＲに適用したが、二次元平面上あるいは三次元空間を移動可能なロボットに適用してもよい。

本実施形態では、関節変数ｑ１〜ｑｎが関節Ｊ_１〜Ｊ_ｎの関節角度を示したが、関節変数の種類はこれに限定されない。例えば、直動関節により連結されたリンク間の距離を示す関節変数を算出するために、本発明に係る逆運動学計算方法及び逆運動学計算装置を利用できる。

また、本実施形態では、ロボットＲの先端Ｔを作業部位としたが、先端部以外の部分を作業部位としてもよい。例えば、リンク同士を接続する関節を作業部位としてもよいし、人型ロボットの頂部を作業部位としてもよい。

本実施形態に係る逆運動学計算装置の構成を示す図である。 図１に示す逆運動学計算装置が適用されるロボットを示す図である。 （ａ）は、評価指標値Ｆ（ｑ）の変動状況の一例を示すグラフであり、（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の別の例を示すグラフである。 図１に示す逆運動学計算装置の処理を示すフローチャートである。

符号の説明

１…逆運動学計算装置、２…入力部、３…変化量計算部、４…評価部、５…収束判定部、６…出力部

Claims (4)

1. 複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、
   制御変数を決定する制御変数決定ステップと、
   前記制御変数決定ステップにおいて決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップであって、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行する、該関数変数算出ステップとを含み、
   前記制御変数決定ステップでは、
   前記関節変数を評価する所定の評価指標値を算出するための評価関数を用いて、前記関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、
   算出された複数の前記評価指標値の変化量に基づいて、該変化量が正から負、又は負から正に切り替わる回数を取得し、取得した該回数に基づいて前記制御変数を決定する、
   ことを特徴とする逆運動学計算方法。
2. 前記関節変数算出ステップでは、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
   ことを特徴とする請求項１に記載の逆運動学計算方法。
3. 複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、
   制御変数を決定する制御変数決定手段と、
   前記制御変数決定手段により決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段であって、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行する、該関節変数算出手段とを備え、
   前記制御変数決定手段が、
   前記関節変数を評価する所定の評価指標値を算出するための評価関数を用いて、前記関節変数算出手段により算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、
   算出された複数の前記評価指標値の変化量に基づいて、該変化量が正から負、又は負から正に切り替わる回数を取得し、取得した該回数に基づいて前記制御変数を決定する、
   ことを特徴とする逆運動学計算装置。
4. 前記関節変数算出手段は、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
   ことを特徴とする請求項３に記載の逆運動学計算装置。

Images