逆運動学計算方法及び逆運動学計算装置
【課題】ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出すること。
【解決手段】複数の関節Ｊ１〜Ｊｎを有するロボットＲの先端Ｔの位置及び姿勢から関節変数ｑ１〜ｑｎを計算する場合、まず、制御変数λを決定する。続いて、決定された制御変数λに基づいて関節変数ベクトルｑｋの変化量Δｑｋを算出し、算出された変化量Δｑｋに基づいてロボットＲの先端Ｔの位置及び姿勢に対応する関節変数ベクトルｑｋ＋１を算出する。制御変数λは、関節変数ベクトルｑｋを評価する所定の評価指標値Ｆ（ｑｋ）の変動状況に基づいて決定される。
【解決手段】複数の関節Ｊ１〜Ｊｎを有するロボットＲの先端Ｔの位置及び姿勢から関節変数ｑ１〜ｑｎを計算する場合、まず、制御変数λを決定する。続いて、決定された制御変数λに基づいて関節変数ベクトルｑｋの変化量Δｑｋを算出し、算出された変化量Δｑｋに基づいてロボットＲの先端Ｔの位置及び姿勢に対応する関節変数ベクトルｑｋ＋１を算出する。制御変数λは、関節変数ベクトルｑｋを評価する所定の評価指標値Ｆ（ｑｋ）の変動状況に基づいて決定される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節を有するロボット（多関節ロボット）の関節変数を計算する逆運動学計算方法及び逆運動学計算装置に関するものである。
【背景技術】
【０００２】
従来から、多関節ロボットの作業部位の位置及び姿勢を指定した上で、その位置及び姿勢を実現するための関節角度を計算する逆運動学計算方法が知られている。例えば、下記特許文献１には、多関節リンク機構の逆運動学解析の効率化を実現するための手法が開示されている。下記特許文献１に記載の逆運動学解析方法は、隣接リンク間の機構学的関係を示す関係式と最終リンクの位置及び姿勢とが与えられた場合に、非線形特性を模擬する制御変数である補正係数をガウス・ニュートン法に導入し、目的関数が単調減少するようにその補正係数を変えつつ反復計算を行うことで関節角度を算出する。
【特許文献１】特開平７−１４６７０３号公報
【特許文献２】特開平７−３６５１８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記特許文献１に記載の手法では、補正係数（制御変数）を経験的に決定しているに過ぎないので、ロボットの機構を定義するパラメータを変更した際に所望の計算結果が得られない可能性がある。例えば、算出される関節角度が局所解に陥るなどして、所望の結果とは異なる計算結果に収束することがある。
【０００４】
本発明は、上記課題を解決するためになされたものであり、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出することが可能な逆運動学計算方法及び逆運動学計算装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
本発明の逆運動学計算方法は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、制御変数を決定する制御変数決定ステップと、制御変数決定ステップにおいて決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップとを含み、制御変数決定ステップでは、関節変数算出ステップにおいて算出された関節変数を評価する所定の評価指標値の変動状況に基づいて制御変数を決定する、ことを特徴とする。
【０００６】
また、本発明の逆運動学計算装置は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、制御変数を決定する制御変数決定手段と、制御変数決定手段により決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段とを備え、制御変数決定手段は、関節変数算出手段により算出された関節変数を評価する所定の評価指標値の変動状況に基づいて制御変数を決定する、ことを特徴とする。
【０００７】
このような逆運動学計算方法及び逆運動学計算装置によれば、制御変数に基づいて関節変数の変化量が算出され、この変化量に基づいて関節変数が算出される。関節変数の算出において用いられる制御変数は、関節変数の算出を安定化する（関節変数の発散を抑制する）ために用いられるパラメータであり、関節変数を評価する所定の評価指標値の変動状況に基づいて決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数が局所解に陥ることを防止することが可能になる。
【０００８】
本発明の逆運動学計算方法において、関節変数算出ステップでは、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行し、制御変数決定ステップでは、評価指標値を算出するための評価関数を用いて、関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出することが好ましい。
【０００９】
また、本発明の逆運動学計算装置において、関節変数算出手段は、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行し、制御変数決定手段は、評価指標値を算出するための評価関数を用いて、関節変数算出手段により算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出することが好ましい。
【００１０】
この場合、関節変数の算出が反復実行され、その結果得られた複数の関節変数のそれぞれについて評価指標値が算出される。この評価指標値は、所定の評価関数を用いて算出される。そして、算出された複数の評価指標値の変化量に基づいて評価指標値の変動状況が導出され、導出された評価指標値の変動状況に基づいて制御変数が決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数の算出を安定化できる（関節変数の発散を抑制できる）。更に、評価指標値の変動状況は、複数の評価指標値の変化量に基づいて導出されるので、より適切な制御変数を決定できる。
【００１１】
本発明の逆運動学計算方法において、関節変数算出ステップでは、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。
【００１２】
また、本発明の逆運動学計算装置において、関節変数算出手段は、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。
【００１３】
この場合、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量が算出される。まず、ガウス・ニュートン法を利用することで、関節変数ベクトルの算出の安定化を実現できる。また、ガウス・ニュートン法に導入される制御変数は、評価指標値の変動状況に基づいて決定されるので、算出対象の関節変数が局所解に陥ることを防止し、所望の関節変数を算出することが可能になる。
【発明の効果】
【００１４】
このような逆運動学計算方法及び逆運動学計算装置によれば、関節変数の算出において用いられる制御変数が、該関節変数を評価する所定の評価指標値の変動状況に基づいて決定されるので、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。
【発明を実施するための最良の形態】
【００１５】
以下、添付図面を参照しながら本発明の実施形態を詳細に説明する。本実施の形態では、本発明に係る逆運動学計算装置を、所定の位置に固定された多関節ロボットの関節角度を計算する逆運動学計算装置１に適用する。なお、図面の説明において同一又は同等の要素には同一の符号を付し、重複する説明を省略する。
【００１６】
図１〜３を参照して、本実施の形態に係る逆運動学計算装置１について説明する。図１は、本実施形態に係る逆運動学計算装置１の構成を示す図である。図２は、図１に示す逆運動学計算装置１が適用されるロボットＲを模式的に示す図である。図３（ａ）及び図３（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の例を示すグラフである。
【００１７】
逆運動学計算装置１は、複数の関節を有するロボットの先端（作業部位）の位置及び姿勢（以下「先端位置及び先端姿勢」ともいう）の入力を受け付け、逆運動学計算により、入力された先端位置及び先端姿勢から各関節の関節角度を算出する。特に、逆運動学計算装置１は、制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいて目標の関節変数を算出する。このとき、逆運動学計算装置１は、算出された関節変数を評価する所定の評価指標値の変動状況に基づいて上記制御変数を決定する。
【００１８】
このために逆運動学計算装置１は、入力部２、変化量計算部３、評価部４、収束判定部５、及び出力部６を備えている。逆運動学計算装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、変化量計算部３、評価部４及び収束判定部５は、ハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。なお、本実施形態では、変化量計算部３が特許請求の範囲に記載する関節変数算出手段に相当し、評価部４が特許請求の範囲に記載する制御変数決定手段に相当する。
【００１９】
まず、逆運動学計算装置１が適用されるロボットＲについて説明する。図２に示すように、ロボットＲは、ｎ個の関節Ｊ１〜Ｊｎと、関節同士を接続するリンクＬ１〜Ｌｎ＋１と、先端のリンクＬｎ＋１の一端Ｔに取り付けられたハンドＨとを備えている。このロボットＲは、末端のリンクＬ１の一端がベースＢに取り付けられることで固定されている。各関節Ｊ１〜Ｊｎにはアクチュエータが内蔵されている。このアクチュエータによって各関節Ｊ１〜Ｊｎがそれぞれ回転し、隣接する二本のリンク間の角度θ１〜θｎが変更される。
【００２０】
逆運動学計算装置１は、ロボットＲの先端Ｔの位置及び姿勢（ハンドＨの位置及び姿勢）から各関節Ｊ１〜Ｊｎの角度θ１〜θｎを決定する装置である。ロボットＲの位置及び姿勢は、ｎ個の関節変数が作る関節空間からデカルト空間への非線形写像と考えられる。ロボットＲの先端位置及び先端姿勢から各関節の角度を決定するということは、この非線形写像の逆像の中から解を見つけることである。
【００２１】
逆運動学計算装置１の各機能的構成要素について説明する前に、この逆運動学計算装置１が用いる理論について説明する。
【００２２】
関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数（関節角度を示す関節変数）をそれぞれｑ１〜ｑｎとすると、関節変数ベクトルはｑ＝（ｑ１，ｑ２，…，ｑｎ）Ｔ、すなわち（ｑ１，ｑ２，…，ｑｎ）の転置行列で表される。このとき、ロボットＲの先端位置（ハンドＨの位置）はＸ（ｑ），Ｙ（ｑ），Ｚ（ｑ）で表され、ロボットＲの先端姿勢（ハンドＨの姿勢）を定義する成分はＰ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）で表される。また、ロボットＲの先端Ｔの目標位置をＸｇ，Ｙｇ，Ｚｇとし、ロボットＲの先端Ｔの目標姿勢を定義する成分をＰ１ｇ，Ｐ２ｇ，…Ｐｍｇとする。
【００２３】
ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑ），Ｙ（ｑ），Ｚ（ｑ），Ｐ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）は、同次変換行列０Ｔｎの各成分座標である。同次変換行列０Ｔｎとは、ベースＢの座標系Σ０から見たハンドＨの座標系Σｎを表す行列であり、以下のように表される。なお、式（１）ではｍ＝９であるが、ｍが他の値（例えば２，４，６など）であってもよい。
【数１】
【００２４】
導出された先端位置及び先端姿勢と目標位置及び目標姿勢との差を以下の式で表す。すなわち、ロボットＲの先端位置とその目標値との差をｆ１，ｆ２，ｆ３とし、ロボットＲの先端姿勢を定義する成分とその目標値との差をｆ４，ｆ５，…，ｆｍ＋３で表す。
【数２】
【００２５】
式（２）で示されるｆ１，ｆ２，…，ｆｍ＋３をまとめ、ｆ（ｑ）＝（ｆ１，ｆ２，…，ｆｍ＋３）Ｔを定義する。なお、（ｆ１，ｆ２，…，ｆｍ＋３）Ｔは、（ｆ１，ｆ２，…，ｆｍ＋３）の転置行列である。更に、評価指標値Ｆ（ｑ）を算出するための評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）を定義する。この評価指標値Ｆ（ｑ）が０になれば、ロボットＲの先端Ｔの目標位置及び目標姿勢を実現する各関節Ｊ１〜Ｊｎの角度θ１〜θｎが算出されたことになる。
【００２６】
この評価指標値Ｆ（ｑ）が０になるようなｆ（ｑ）を算出するために、関節変数ベクトルｑとして適当な値ｑｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））Ｔを与える。続いて、このｑｋを式（２）に適用することでｆ（ｑｋ）を算出し、算出されたｆ（ｑｋ）を評価関数に当てはめることで評価指標値Ｆ（ｑｋ）を算出する。
【００２７】
続いて、ガウス・ニュートン法の理論を利用して、関節変数ベクトルの変化量Δｑｋを導出する。すなわち、下記式（３）を満たすΔｑｋを導出する。式（３）において、Ｊはヤコビ行列（Ｊ＝｛∂ｆｉ／∂ｑｉ｝）であり、Ｉは単位行列である。また、λは補正操作のための制御変数であり、正数値である。なお、関節変数ベクトルｑｋの変化量Δｑｋを算出するということは、関節変数ｑ１〜ｑｎの変化量を算出するということである。
（数３）
（ＪＴ・Ｊ＋λＩ）Δｑｋ＝−ＪＴ（ｑｋ）ｆ（ｑｋ） …（３）
【００２８】
式（３）において制御変数λを０とすれば、式（３）はガウス・ニュートン法そのものを表す。ガウス・ニュートン法をそのまま利用した場合（制御変数λを導入しない場合）、関節変数ベクトルｑの設定によっては、関節変数ベクトルが発散することがある。また、行列ＪＴ・Ｊが正則でない場合には解が無数に存在してしまうなどの問題が発生する。そこで、行列ＪＴ・Ｊの対角項に正数値を加える操作を施す。このように制御変数λを導入したガウス・ニュートン法を用いることで、関節変数ベクトルの算出の安定化を図る。
【００２９】
ただし、制御変数λを固定した場合には、関節変数ベクトルが局所解に陥り、先端Ｔの
目標位置及び目標姿勢を実現する関節角度θ１〜θｎを得られないことがある。そこで、逆運動学計算装置１は、所定の演算を行なうことで制御変数λを自動的に更新する。制御変数λの更新方法については後述する。
【００３０】
変化量Δｑｋが決まると、下記式（４）により新たな関節変数ベクトルｑｋ＋１が算出される。
（数４）
ｑｋ＋１＝ｑｋ＋Δｑｋ …（４）
【００３１】
例えば、関節変数ベクトルの初期値ｑ０＝（ｑ１（０），ｑ２（０），…，ｑｎ（０））Ｔを式（２）に代入することでｆ（ｑ０）が算出され、更に評価指標値Ｆ（ｑ０）が算出可能となる。また、式（３）により関節変数ベクトルｑ０に対する変化量Δｑ０が算出される。続いて、式（４）にｑ０及びΔｑ０を代入することで新たな関節変数ベクトルｑ１＝ｑ０＋Δｑ０が算出される。
【００３２】
次に、算出された関節変数ベクトルｑ１を式（２）に適用することでｆ（ｑ１）が算出され、これにより評価指標値Ｆ（ｑ１）が算出可能となる。また、式（３）により関節変数ベクトルｑ１に対する変化量Δｑ１が算出され、式（４）により新たな関節変数ベクトルｑ２が算出される。以後、このような演算を繰り返すことで、Ｆ（ｑｋ）＝０となるような関節変数ベクトルｑｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））Ｔが算出され、先端Ｔの目標位置及び目標姿勢を実現する関節角度θ１〜θｎが導出される。
【００３３】
次に、逆運動学計算装置１の各機能的構成要素について説明する。
【００３４】
入力部２は、逆運動学計算に必要な値の入力を受け付ける手段である。値の入力は、オペレータまたはエンコーダによって行われる。入力部２により受け付けられる値は、ベースＢの位置（ロボットＲの立ち位置）、先端Ｔの目標位置Ｘｇ，Ｙｇ，Ｚｇ、先端Ｔの目標姿勢を定義する成分Ｐ１ｇ，Ｐ２ｇ，…Ｐｍｇ、関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ０、及び一回の収束判定までに関節変数ベクトルを繰り返し算出する回数（評価データ数）Ｓである。入力部２は、受け付けた値を変化量計算部３に出力する。
【００３５】
変化量計算部３は、入力部２又は後述する収束判定部５からの入力値を式（２）〜（４）に適用することでＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を導出する手段である。このために変化量計算部３は、式（２）〜（４）が規定されているプログラムを予めメモリ（図示せず）から読み出す。変化量計算部３は、評価データ数Ｓの分だけ式（２）〜（４）の演算を繰り返すことにより、関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を算出する。変化量計算部３は、導出されたＳ個の関節変数ベクトルｑｋと、式（３）の演算で用いた制御変数λとを評価部４に出力する。
【００３６】
評価部４は、変化量計算部３から入力されたＳ個の関節変数ベクトルｑｋに基づいて式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）を計算することでＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）を算出し、これら複数の評価指標値Ｆ（ｑｋ）の変動状況を取得する手段である。このために評価部４は、式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）が規定されているプログラムを予めメモリ（図示せず）から読み出す。具体的には、評価部４は、算出したＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）のそれぞれについてＦ（ｑｋ）からＦ（ｑｋ＋１）への変化量ΔＦｋが正の値であるか負の値であるかを検査し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、下記式（５）を用いて制御変数λを更新（決定）する。
（数５）
λ＝λ０（１＋ｊ／Ｓ） …（５）
【００３７】
このような評価部４の処理を、図３（ａ）及び（ｂ）を用いて具体的に説明する。なお、図３（ａ）及び（ｂ）のグラフにおいて、縦軸は評価指標値Ｆ（ｑ）であり、横軸は関節変数ベクトルである。図３（ａ）及び（ｂ）に示す例では、評価データ数Ｓが５に設定されているものとする。
【００３８】
図３（ａ）において、Ｆ（ｑ１）からＦ（ｑ２）への変化量ΔＦ１は負の値であるが、Ｆ（ｑ２）からＦ（ｑ３）への変化量ΔＦ２は正の値である。すなわち、Ｆ（ｑ１）からＦ（ｑ２）にかけては評価指標値Ｆ（ｑ）が減少しているが、Ｆ（ｑ２）からＦ（ｑ３）にかけては増加している。同様に、変化量ΔＦ３は負の値であり、変化量ΔＦ４は正の値である。したがって、ΔＦ１からΔＦ２にかけては変化量が負の値から正の値に切り替わり、ΔＦ２からΔＦ３にかけては変化量が正の値から負の値に切り替わり、ΔＦ３からΔＦ４にかけては変化量が負の値から正の値に切り替わっている。すなわち、切替り回数ｊは３である。この場合、制御変数λは、λ０（１＋３／５）＝１．６λ０となる。
【００３９】
これに対して図３（ｂ）の場合、変化量ΔＦｋ（ｋ＝１〜４）はすべて負の値であるから、切替り回数ｊは０である。この場合、制御変数λは、λ０（１＋０／５）＝λ０となる。
【００４０】
すなわち、評価部４は、評価指標値Ｆ（ｑ）が増加と減少とを繰り返すなどしてその変動が大きい場合には制御変数λを大きくし、評価指標値Ｆ（ｑ）が単調減少するなどしてその変動が小さい場合には制御変数λを小さくする。このように制御変数λを更新する理由は、評価指標値Ｆ（ｑ）の変動が大きいほど制御変数λを大きくして関節変数ベクトルの変化量Δｑｋを小さく抑えることで、関節変数ベクトルｑｋの不安定な挙動を回避するためである。
【００４１】
評価部４は、更新された制御変数λと、Ｓ回目に算出された評価指標値Ｆ（ｑＳ）と、Ｓ−１回目及びＳ回目に算出された関節変数ベクトルｑＳ，ｑＳ−１とを収束判定部５に出力する。
【００４２】
収束判定部５は、入力された評価指標値Ｆ（ｑＳ）が所定の正数値αよりも小さいか、又は関節変数ベクトルの変化量ΔｑＳ−１＝ｑＳ−ｑＳ−１が所定の正数値βよりも小さい場合に、ロボットＲの各関節Ｊ１〜Ｊｎの角度を示す関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔが目標値に到達したと判定する手段である。言い換えれば、収束判定部５は、ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑＳ），Ｙ（ｑＳ），Ｚ（ｑＳ），Ｐ１（ｑＳ），Ｐ２（ｑＳ），…，Ｐｍ（ｑＳ）がＸｇ，Ｙｇ，Ｚｇ，Ｐ１ｇ，Ｐ２ｇ，…Ｐｍｇに到達したと判定する。この場合、収束判定部５は、関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔを出力部６に出力する。
【００４３】
これに対し、入力された評価指標値Ｆ（ｑＳ）が正数値α以上であり、且つ入力された関節変数ベクトルの変化量ΔｑＳ−１が正数値β以上である場合、収束判定部５は、関節変数ベクトルｑを再計算すると判定し、入力された更新後の制御変数λと、関節変数ベクトルｑＳとを変化量計算部３に出力する。
【００４４】
収束判定部５は、このような処理を行うために、所定の正数値α，βを予めメモリ（図示せず）から読み出す。本来、収束判定部５は、正数値αとして０を読み出し、入力された評価指標値Ｆ（ｑＳ）が０である場合に関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔが目標値に到達したと判定するのが好ましい。しかし、評価指標値Ｆ（ｑＳ）が０か否かのみに基づいて関節変数ベクトルｑＳの収束の有無を判定すると、関節変数ベクトルｑＳが局所解に陥ってしまい、逆運動学計算装置１による処理がいわゆる無限ループに陥る可能性がある。そこで、収束判定部５は、二種類の正数値α，βを用いて収束判定を行なう。
【００４５】
出力部６は、収束判定部５から入力された関節変数ベクトルｑＳを、ロボットＲを動作させる制御部（図示せず）に出力する手段である。出力部６から図示しない制御部に関節変数ベクトルｑＳが出力させることで、ロボットＲの各関節Ｊ１〜Ｊｎのアクチュエータが作動し、各関節Ｊ１〜Ｊｎの角度θ１〜θｎが変更される。その結果、ハンドＨの位置及び姿勢が目標とするものに変更される。
【００４６】
次に、図４を用いて、図１に示す逆運動学計算装置１の処理を説明するとともに本実施形態に係る逆運動学計算方法について説明する。図４は、逆運動学計算装置１の処理を示すフローチャートである。
【００４７】
逆運動学計算装置１では、まず、入力部２が、オペレータ又はエンコーダから逆運動学計算に必要な値の入力を受け付ける（ステップＳ１１）。具体的には、入力部２は、ベースＢの位置、ロボットＲの先端Ｔの目標位置及び目標姿勢、関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ０、及び評価データ数Ｓの入力を受け付ける。
【００４８】
続いて変化量計算部３が、入力された関節変数の初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、及び制御変数λ０を用いて、反復計算によりＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を算出する（ステップＳ１２、関節変数算出ステップ）。具体的には、変化量計算部３は、関節変数の初期値に基づいて導出されたロボットＲの先端位置及び先端姿勢と目標とする先端位置及び先端姿勢との差ｆ１，ｆ２，…，ｆｍ＋３を式（２）により算出する。続いて変化量計算部３は、算出されたｆ１，ｆ２，…，ｆｍ＋３に基づいてｆ（ｑｋ）を定義し、そのｆ（ｑｋ）を制御変数の初期値λ０を導入したガウス・ニュートン法に当てはめることで関節変数の変化量Δｑｋを導出する。そして、変化量計算部３は、その変化量Δｑｋと関節変数ベクトルｑｋを式（４）に当てはめることで次の関節変数ベクトルｑｋ＋１を導出する。変化量計算部３は、式（２）〜（４）を用いたこのような処理を評価データ数Ｓの分だけ繰り返す。
【００４９】
続いて評価部４が、導出されたＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）に基づいてＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）を導出し（ステップＳ１３、制御変数決定ステップ）、評価指標値Ｆ（ｑｋ）の変動状況を取得する（ステップＳ１４、制御変数決定ステップ）。具体的には、評価部４は、Ｓ個の評価指標値Ｆ（ｑｋ）のそれぞれについてＦ（ｑｋ）からＦ（ｑｋ＋１）への変化量ΔＦｋを算出し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、評価データ数Ｓ、制御変数の初期値λ０、及び導出された切替り回数ｊを式（５）に当てはめることで制御変数λを更新する（ステップＳ１５、制御変数決定ステップ）。言い換えれば、評価部４は、評価指標値Ｆ（ｑｋ）の変動状況に基づいて制御変数を決定する。
【００５０】
続いて収束判定部５が、評価指標値Ｆ（ｑＳ）が所定の正数値αよりも小さいか又は関節変数ベクトルの変化量ΔｑＳ−１＝ｑＳ−ｑＳ−１が所定の正数値βよりも小さいか否かを判定する（ステップＳ１６）。ここでＦ（ｑＳ）＜α又はΔｑＳ−１＜βである場合（ステップＳ１６；ＹＥＳ）、収束判定部５は、関節変数ベクトルｑＳが目標値に到達した、すなわちロボットＲの先端の目標位置及び目標姿勢を実現する各関節Ｊ１〜Ｊｎの角度θ１〜θｎが導出されたと判定する。そして、収束判定部５は、関節変数ベクトルｑＳを出力部６に出力する。その後、出力部６が導出された関節変数ベクトルｑＳをロボットＲを動作させる制御部（図示せず）に出力する（ステップＳ１７）。これにより、各関節Ｊ１〜Ｊｎのアクチュエータが作動して各関節の角度θ１〜θｎが変わり、先端Ｔ（ハンドＨ）の位置及び姿勢が目標とするものに変更される。
【００５１】
これに対し、ここでＦ（ｑＳ）≧α且つΔｑＳ−１≧βである場合（ステップＳ１６；ＮＯ）、収束判定部５は、入力された更新後の制御変数λと関節変数ベクトルｑＳとを変化量計算部３に出力する。これにより、今度はｑＳを初期値として、ステップＳ１２以降の処理が実行される。なお、ステップＳ１２における計算では、更新後の制御変数λを導入したガウス・ニュートン法が用いられる。
【００５２】
以上説明したように、本実施形態によれば、制御変数λが、経験的に決められるのではなく評価指標値Ｆ（ｑ）の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数λを決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数λを評価指標値Ｆ（ｑ）の変動状況に基づいて決定することで、関節変数ベクトルｑｋの算出を安定化する（関節変数ベクトルｑｋの発散を抑制する）とともに、関節変数ベクトルｑｋが局所解に陥ることを防止することが可能になる。
【００５３】
また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況は、評価データ数Ｓの分だけ算出された評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）の変化量Ｆｋ（ｋ＝１〜Ｓ−１）に基づいて導出されるので、より適切な制御変数を決定できる。
【００５４】
また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況に基づいて決定される制御変数λを導入したガウス・ニュートン法を用いることで（式（３）を用いることで）、算出対象の関節変数ベクトルｑ（関節変数ｑ１〜ｑｎ）が局所解に陥ることを防止し、所望の関節変数ベクトル（関節変数）を算出することが可能になる。
【００５５】
以上、本発明をその実施形態に基づいて詳細に説明した。しかし、本発明は上記実施形態に限定されるものではない。本発明は、その要旨を逸脱しない範囲で以下のような様々な変形が可能である。
【００５６】
本実施形態では、本発明に係る逆運動学計算装置を、ベースＢに固定され多数の関節を有するロボットＲに適用したが、二次元平面上あるいは三次元空間を移動可能なロボットに適用してもよい。
【００５７】
本実施形態では、関節変数ｑ１〜ｑｎが関節Ｊ１〜Ｊｎの関節角度を示したが、関節変数の種類はこれに限定されない。例えば、直動関節により連結されたリンク間の距離を示す関節変数を算出するために、本発明に係る逆運動学計算方法及び逆運動学計算装置を利用できる。
【００５８】
また、本実施形態では、ロボットＲの先端Ｔを作業部位としたが、先端部以外の部分を作業部位としてもよい。例えば、リンク同士を接続する関節を作業部位としてもよいし、人型ロボットの頂部を作業部位としてもよい。
【図面の簡単な説明】
【００５９】
【図１】本実施形態に係る逆運動学計算装置の構成を示す図である。
【図２】図１に示す逆運動学計算装置が適用されるロボットを示す図である。
【図３】（ａ）は、評価指標値Ｆ（ｑ）の変動状況の一例を示すグラフであり、（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の別の例を示すグラフである。
【図４】図１に示す逆運動学計算装置の処理を示すフローチャートである。
【符号の説明】
【００６０】
１…逆運動学計算装置、２…入力部、３…変化量計算部、４…評価部、５…収束判定部、６…出力部
【技術分野】
【０００１】
本発明は、複数の関節を有するロボット（多関節ロボット）の関節変数を計算する逆運動学計算方法及び逆運動学計算装置に関するものである。
【背景技術】
【０００２】
従来から、多関節ロボットの作業部位の位置及び姿勢を指定した上で、その位置及び姿勢を実現するための関節角度を計算する逆運動学計算方法が知られている。例えば、下記特許文献１には、多関節リンク機構の逆運動学解析の効率化を実現するための手法が開示されている。下記特許文献１に記載の逆運動学解析方法は、隣接リンク間の機構学的関係を示す関係式と最終リンクの位置及び姿勢とが与えられた場合に、非線形特性を模擬する制御変数である補正係数をガウス・ニュートン法に導入し、目的関数が単調減少するようにその補正係数を変えつつ反復計算を行うことで関節角度を算出する。
【特許文献１】特開平７−１４６７０３号公報
【特許文献２】特開平７−３６５１８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記特許文献１に記載の手法では、補正係数（制御変数）を経験的に決定しているに過ぎないので、ロボットの機構を定義するパラメータを変更した際に所望の計算結果が得られない可能性がある。例えば、算出される関節角度が局所解に陥るなどして、所望の結果とは異なる計算結果に収束することがある。
【０００４】
本発明は、上記課題を解決するためになされたものであり、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出することが可能な逆運動学計算方法及び逆運動学計算装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
本発明の逆運動学計算方法は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、制御変数を決定する制御変数決定ステップと、制御変数決定ステップにおいて決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップとを含み、制御変数決定ステップでは、関節変数算出ステップにおいて算出された関節変数を評価する所定の評価指標値の変動状況に基づいて制御変数を決定する、ことを特徴とする。
【０００６】
また、本発明の逆運動学計算装置は、複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、制御変数を決定する制御変数決定手段と、制御変数決定手段により決定された制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいてロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段とを備え、制御変数決定手段は、関節変数算出手段により算出された関節変数を評価する所定の評価指標値の変動状況に基づいて制御変数を決定する、ことを特徴とする。
【０００７】
このような逆運動学計算方法及び逆運動学計算装置によれば、制御変数に基づいて関節変数の変化量が算出され、この変化量に基づいて関節変数が算出される。関節変数の算出において用いられる制御変数は、関節変数の算出を安定化する（関節変数の発散を抑制する）ために用いられるパラメータであり、関節変数を評価する所定の評価指標値の変動状況に基づいて決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数が局所解に陥ることを防止することが可能になる。
【０００８】
本発明の逆運動学計算方法において、関節変数算出ステップでは、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行し、制御変数決定ステップでは、評価指標値を算出するための評価関数を用いて、関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出することが好ましい。
【０００９】
また、本発明の逆運動学計算装置において、関節変数算出手段は、関節変数の変化量の算出と関節変数の算出とを所定の回数だけ反復して実行し、制御変数決定手段は、評価指標値を算出するための評価関数を用いて、関節変数算出手段により算出された複数の関節変数のそれぞれについて評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出することが好ましい。
【００１０】
この場合、関節変数の算出が反復実行され、その結果得られた複数の関節変数のそれぞれについて評価指標値が算出される。この評価指標値は、所定の評価関数を用いて算出される。そして、算出された複数の評価指標値の変化量に基づいて評価指標値の変動状況が導出され、導出された評価指標値の変動状況に基づいて制御変数が決定される。このように、制御変数が、経験的に決められるのではなく評価指標値の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数を決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数を評価指標値の変動状況に基づいて決定することで、関節変数の算出を安定化できる（関節変数の発散を抑制できる）。更に、評価指標値の変動状況は、複数の評価指標値の変化量に基づいて導出されるので、より適切な制御変数を決定できる。
【００１１】
本発明の逆運動学計算方法において、関節変数算出ステップでは、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。
【００１２】
また、本発明の逆運動学計算装置において、関節変数算出手段は、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量を算出することが好ましい。
【００１３】
この場合、制御変数を導入したガウス・ニュートン法を用いて関節変数の変化量が算出される。まず、ガウス・ニュートン法を利用することで、関節変数ベクトルの算出の安定化を実現できる。また、ガウス・ニュートン法に導入される制御変数は、評価指標値の変動状況に基づいて決定されるので、算出対象の関節変数が局所解に陥ることを防止し、所望の関節変数を算出することが可能になる。
【発明の効果】
【００１４】
このような逆運動学計算方法及び逆運動学計算装置によれば、関節変数の算出において用いられる制御変数が、該関節変数を評価する所定の評価指標値の変動状況に基づいて決定されるので、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。
【発明を実施するための最良の形態】
【００１５】
以下、添付図面を参照しながら本発明の実施形態を詳細に説明する。本実施の形態では、本発明に係る逆運動学計算装置を、所定の位置に固定された多関節ロボットの関節角度を計算する逆運動学計算装置１に適用する。なお、図面の説明において同一又は同等の要素には同一の符号を付し、重複する説明を省略する。
【００１６】
図１〜３を参照して、本実施の形態に係る逆運動学計算装置１について説明する。図１は、本実施形態に係る逆運動学計算装置１の構成を示す図である。図２は、図１に示す逆運動学計算装置１が適用されるロボットＲを模式的に示す図である。図３（ａ）及び図３（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の例を示すグラフである。
【００１７】
逆運動学計算装置１は、複数の関節を有するロボットの先端（作業部位）の位置及び姿勢（以下「先端位置及び先端姿勢」ともいう）の入力を受け付け、逆運動学計算により、入力された先端位置及び先端姿勢から各関節の関節角度を算出する。特に、逆運動学計算装置１は、制御変数に基づいて関節変数の変化量を算出し、算出された変化量に基づいて目標の関節変数を算出する。このとき、逆運動学計算装置１は、算出された関節変数を評価する所定の評価指標値の変動状況に基づいて上記制御変数を決定する。
【００１８】
このために逆運動学計算装置１は、入力部２、変化量計算部３、評価部４、収束判定部５、及び出力部６を備えている。逆運動学計算装置１の主要部はコンピュータ上あるいはロボット内の電子制御ユニットなどに構成され、特に、変化量計算部３、評価部４及び収束判定部５は、ハードディスクあるいはＲＯＭ内に格納された各アプリケーションプログラムをＲＡＭにロードし、ＣＰＵで実行することによって構成される。なお、本実施形態では、変化量計算部３が特許請求の範囲に記載する関節変数算出手段に相当し、評価部４が特許請求の範囲に記載する制御変数決定手段に相当する。
【００１９】
まず、逆運動学計算装置１が適用されるロボットＲについて説明する。図２に示すように、ロボットＲは、ｎ個の関節Ｊ１〜Ｊｎと、関節同士を接続するリンクＬ１〜Ｌｎ＋１と、先端のリンクＬｎ＋１の一端Ｔに取り付けられたハンドＨとを備えている。このロボットＲは、末端のリンクＬ１の一端がベースＢに取り付けられることで固定されている。各関節Ｊ１〜Ｊｎにはアクチュエータが内蔵されている。このアクチュエータによって各関節Ｊ１〜Ｊｎがそれぞれ回転し、隣接する二本のリンク間の角度θ１〜θｎが変更される。
【００２０】
逆運動学計算装置１は、ロボットＲの先端Ｔの位置及び姿勢（ハンドＨの位置及び姿勢）から各関節Ｊ１〜Ｊｎの角度θ１〜θｎを決定する装置である。ロボットＲの位置及び姿勢は、ｎ個の関節変数が作る関節空間からデカルト空間への非線形写像と考えられる。ロボットＲの先端位置及び先端姿勢から各関節の角度を決定するということは、この非線形写像の逆像の中から解を見つけることである。
【００２１】
逆運動学計算装置１の各機能的構成要素について説明する前に、この逆運動学計算装置１が用いる理論について説明する。
【００２２】
関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数（関節角度を示す関節変数）をそれぞれｑ１〜ｑｎとすると、関節変数ベクトルはｑ＝（ｑ１，ｑ２，…，ｑｎ）Ｔ、すなわち（ｑ１，ｑ２，…，ｑｎ）の転置行列で表される。このとき、ロボットＲの先端位置（ハンドＨの位置）はＸ（ｑ），Ｙ（ｑ），Ｚ（ｑ）で表され、ロボットＲの先端姿勢（ハンドＨの姿勢）を定義する成分はＰ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）で表される。また、ロボットＲの先端Ｔの目標位置をＸｇ，Ｙｇ，Ｚｇとし、ロボットＲの先端Ｔの目標姿勢を定義する成分をＰ１ｇ，Ｐ２ｇ，…Ｐｍｇとする。
【００２３】
ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑ），Ｙ（ｑ），Ｚ（ｑ），Ｐ１（ｑ），Ｐ２（ｑ），…，Ｐｍ（ｑ）は、同次変換行列０Ｔｎの各成分座標である。同次変換行列０Ｔｎとは、ベースＢの座標系Σ０から見たハンドＨの座標系Σｎを表す行列であり、以下のように表される。なお、式（１）ではｍ＝９であるが、ｍが他の値（例えば２，４，６など）であってもよい。
【数１】
【００２４】
導出された先端位置及び先端姿勢と目標位置及び目標姿勢との差を以下の式で表す。すなわち、ロボットＲの先端位置とその目標値との差をｆ１，ｆ２，ｆ３とし、ロボットＲの先端姿勢を定義する成分とその目標値との差をｆ４，ｆ５，…，ｆｍ＋３で表す。
【数２】
【００２５】
式（２）で示されるｆ１，ｆ２，…，ｆｍ＋３をまとめ、ｆ（ｑ）＝（ｆ１，ｆ２，…，ｆｍ＋３）Ｔを定義する。なお、（ｆ１，ｆ２，…，ｆｍ＋３）Ｔは、（ｆ１，ｆ２，…，ｆｍ＋３）の転置行列である。更に、評価指標値Ｆ（ｑ）を算出するための評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）を定義する。この評価指標値Ｆ（ｑ）が０になれば、ロボットＲの先端Ｔの目標位置及び目標姿勢を実現する各関節Ｊ１〜Ｊｎの角度θ１〜θｎが算出されたことになる。
【００２６】
この評価指標値Ｆ（ｑ）が０になるようなｆ（ｑ）を算出するために、関節変数ベクトルｑとして適当な値ｑｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））Ｔを与える。続いて、このｑｋを式（２）に適用することでｆ（ｑｋ）を算出し、算出されたｆ（ｑｋ）を評価関数に当てはめることで評価指標値Ｆ（ｑｋ）を算出する。
【００２７】
続いて、ガウス・ニュートン法の理論を利用して、関節変数ベクトルの変化量Δｑｋを導出する。すなわち、下記式（３）を満たすΔｑｋを導出する。式（３）において、Ｊはヤコビ行列（Ｊ＝｛∂ｆｉ／∂ｑｉ｝）であり、Ｉは単位行列である。また、λは補正操作のための制御変数であり、正数値である。なお、関節変数ベクトルｑｋの変化量Δｑｋを算出するということは、関節変数ｑ１〜ｑｎの変化量を算出するということである。
（数３）
（ＪＴ・Ｊ＋λＩ）Δｑｋ＝−ＪＴ（ｑｋ）ｆ（ｑｋ） …（３）
【００２８】
式（３）において制御変数λを０とすれば、式（３）はガウス・ニュートン法そのものを表す。ガウス・ニュートン法をそのまま利用した場合（制御変数λを導入しない場合）、関節変数ベクトルｑの設定によっては、関節変数ベクトルが発散することがある。また、行列ＪＴ・Ｊが正則でない場合には解が無数に存在してしまうなどの問題が発生する。そこで、行列ＪＴ・Ｊの対角項に正数値を加える操作を施す。このように制御変数λを導入したガウス・ニュートン法を用いることで、関節変数ベクトルの算出の安定化を図る。
【００２９】
ただし、制御変数λを固定した場合には、関節変数ベクトルが局所解に陥り、先端Ｔの
目標位置及び目標姿勢を実現する関節角度θ１〜θｎを得られないことがある。そこで、逆運動学計算装置１は、所定の演算を行なうことで制御変数λを自動的に更新する。制御変数λの更新方法については後述する。
【００３０】
変化量Δｑｋが決まると、下記式（４）により新たな関節変数ベクトルｑｋ＋１が算出される。
（数４）
ｑｋ＋１＝ｑｋ＋Δｑｋ …（４）
【００３１】
例えば、関節変数ベクトルの初期値ｑ０＝（ｑ１（０），ｑ２（０），…，ｑｎ（０））Ｔを式（２）に代入することでｆ（ｑ０）が算出され、更に評価指標値Ｆ（ｑ０）が算出可能となる。また、式（３）により関節変数ベクトルｑ０に対する変化量Δｑ０が算出される。続いて、式（４）にｑ０及びΔｑ０を代入することで新たな関節変数ベクトルｑ１＝ｑ０＋Δｑ０が算出される。
【００３２】
次に、算出された関節変数ベクトルｑ１を式（２）に適用することでｆ（ｑ１）が算出され、これにより評価指標値Ｆ（ｑ１）が算出可能となる。また、式（３）により関節変数ベクトルｑ１に対する変化量Δｑ１が算出され、式（４）により新たな関節変数ベクトルｑ２が算出される。以後、このような演算を繰り返すことで、Ｆ（ｑｋ）＝０となるような関節変数ベクトルｑｋ＝（ｑ１（ｋ），ｑ２（ｋ），…，ｑｎ（ｋ））Ｔが算出され、先端Ｔの目標位置及び目標姿勢を実現する関節角度θ１〜θｎが導出される。
【００３３】
次に、逆運動学計算装置１の各機能的構成要素について説明する。
【００３４】
入力部２は、逆運動学計算に必要な値の入力を受け付ける手段である。値の入力は、オペレータまたはエンコーダによって行われる。入力部２により受け付けられる値は、ベースＢの位置（ロボットＲの立ち位置）、先端Ｔの目標位置Ｘｇ，Ｙｇ，Ｚｇ、先端Ｔの目標姿勢を定義する成分Ｐ１ｇ，Ｐ２ｇ，…Ｐｍｇ、関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ０、及び一回の収束判定までに関節変数ベクトルを繰り返し算出する回数（評価データ数）Ｓである。入力部２は、受け付けた値を変化量計算部３に出力する。
【００３５】
変化量計算部３は、入力部２又は後述する収束判定部５からの入力値を式（２）〜（４）に適用することでＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を導出する手段である。このために変化量計算部３は、式（２）〜（４）が規定されているプログラムを予めメモリ（図示せず）から読み出す。変化量計算部３は、評価データ数Ｓの分だけ式（２）〜（４）の演算を繰り返すことにより、関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を算出する。変化量計算部３は、導出されたＳ個の関節変数ベクトルｑｋと、式（３）の演算で用いた制御変数λとを評価部４に出力する。
【００３６】
評価部４は、変化量計算部３から入力されたＳ個の関節変数ベクトルｑｋに基づいて式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）を計算することでＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）を算出し、これら複数の評価指標値Ｆ（ｑｋ）の変動状況を取得する手段である。このために評価部４は、式（２）及び評価関数Ｆ（ｑ）＝ｆ（ｑ）Ｔ・ｆ（ｑ）が規定されているプログラムを予めメモリ（図示せず）から読み出す。具体的には、評価部４は、算出したＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）のそれぞれについてＦ（ｑｋ）からＦ（ｑｋ＋１）への変化量ΔＦｋが正の値であるか負の値であるかを検査し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、下記式（５）を用いて制御変数λを更新（決定）する。
（数５）
λ＝λ０（１＋ｊ／Ｓ） …（５）
【００３７】
このような評価部４の処理を、図３（ａ）及び（ｂ）を用いて具体的に説明する。なお、図３（ａ）及び（ｂ）のグラフにおいて、縦軸は評価指標値Ｆ（ｑ）であり、横軸は関節変数ベクトルである。図３（ａ）及び（ｂ）に示す例では、評価データ数Ｓが５に設定されているものとする。
【００３８】
図３（ａ）において、Ｆ（ｑ１）からＦ（ｑ２）への変化量ΔＦ１は負の値であるが、Ｆ（ｑ２）からＦ（ｑ３）への変化量ΔＦ２は正の値である。すなわち、Ｆ（ｑ１）からＦ（ｑ２）にかけては評価指標値Ｆ（ｑ）が減少しているが、Ｆ（ｑ２）からＦ（ｑ３）にかけては増加している。同様に、変化量ΔＦ３は負の値であり、変化量ΔＦ４は正の値である。したがって、ΔＦ１からΔＦ２にかけては変化量が負の値から正の値に切り替わり、ΔＦ２からΔＦ３にかけては変化量が正の値から負の値に切り替わり、ΔＦ３からΔＦ４にかけては変化量が負の値から正の値に切り替わっている。すなわち、切替り回数ｊは３である。この場合、制御変数λは、λ０（１＋３／５）＝１．６λ０となる。
【００３９】
これに対して図３（ｂ）の場合、変化量ΔＦｋ（ｋ＝１〜４）はすべて負の値であるから、切替り回数ｊは０である。この場合、制御変数λは、λ０（１＋０／５）＝λ０となる。
【００４０】
すなわち、評価部４は、評価指標値Ｆ（ｑ）が増加と減少とを繰り返すなどしてその変動が大きい場合には制御変数λを大きくし、評価指標値Ｆ（ｑ）が単調減少するなどしてその変動が小さい場合には制御変数λを小さくする。このように制御変数λを更新する理由は、評価指標値Ｆ（ｑ）の変動が大きいほど制御変数λを大きくして関節変数ベクトルの変化量Δｑｋを小さく抑えることで、関節変数ベクトルｑｋの不安定な挙動を回避するためである。
【００４１】
評価部４は、更新された制御変数λと、Ｓ回目に算出された評価指標値Ｆ（ｑＳ）と、Ｓ−１回目及びＳ回目に算出された関節変数ベクトルｑＳ，ｑＳ−１とを収束判定部５に出力する。
【００４２】
収束判定部５は、入力された評価指標値Ｆ（ｑＳ）が所定の正数値αよりも小さいか、又は関節変数ベクトルの変化量ΔｑＳ−１＝ｑＳ−ｑＳ−１が所定の正数値βよりも小さい場合に、ロボットＲの各関節Ｊ１〜Ｊｎの角度を示す関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔが目標値に到達したと判定する手段である。言い換えれば、収束判定部５は、ロボットＲの先端位置及び先端姿勢を定義する成分Ｘ（ｑＳ），Ｙ（ｑＳ），Ｚ（ｑＳ），Ｐ１（ｑＳ），Ｐ２（ｑＳ），…，Ｐｍ（ｑＳ）がＸｇ，Ｙｇ，Ｚｇ，Ｐ１ｇ，Ｐ２ｇ，…Ｐｍｇに到達したと判定する。この場合、収束判定部５は、関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔを出力部６に出力する。
【００４３】
これに対し、入力された評価指標値Ｆ（ｑＳ）が正数値α以上であり、且つ入力された関節変数ベクトルの変化量ΔｑＳ−１が正数値β以上である場合、収束判定部５は、関節変数ベクトルｑを再計算すると判定し、入力された更新後の制御変数λと、関節変数ベクトルｑＳとを変化量計算部３に出力する。
【００４４】
収束判定部５は、このような処理を行うために、所定の正数値α，βを予めメモリ（図示せず）から読み出す。本来、収束判定部５は、正数値αとして０を読み出し、入力された評価指標値Ｆ（ｑＳ）が０である場合に関節変数ベクトルｑＳ＝（ｑ１（Ｓ），ｑ２（Ｓ），…，ｑｎ（Ｓ））Ｔが目標値に到達したと判定するのが好ましい。しかし、評価指標値Ｆ（ｑＳ）が０か否かのみに基づいて関節変数ベクトルｑＳの収束の有無を判定すると、関節変数ベクトルｑＳが局所解に陥ってしまい、逆運動学計算装置１による処理がいわゆる無限ループに陥る可能性がある。そこで、収束判定部５は、二種類の正数値α，βを用いて収束判定を行なう。
【００４５】
出力部６は、収束判定部５から入力された関節変数ベクトルｑＳを、ロボットＲを動作させる制御部（図示せず）に出力する手段である。出力部６から図示しない制御部に関節変数ベクトルｑＳが出力させることで、ロボットＲの各関節Ｊ１〜Ｊｎのアクチュエータが作動し、各関節Ｊ１〜Ｊｎの角度θ１〜θｎが変更される。その結果、ハンドＨの位置及び姿勢が目標とするものに変更される。
【００４６】
次に、図４を用いて、図１に示す逆運動学計算装置１の処理を説明するとともに本実施形態に係る逆運動学計算方法について説明する。図４は、逆運動学計算装置１の処理を示すフローチャートである。
【００４７】
逆運動学計算装置１では、まず、入力部２が、オペレータ又はエンコーダから逆運動学計算に必要な値の入力を受け付ける（ステップＳ１１）。具体的には、入力部２は、ベースＢの位置、ロボットＲの先端Ｔの目標位置及び目標姿勢、関節Ｊ１〜Ｊｎのそれぞれに対応する関節変数ｑ１，ｑ２，…，ｑｎの初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、制御変数λの初期値λ０、及び評価データ数Ｓの入力を受け付ける。
【００４８】
続いて変化量計算部３が、入力された関節変数の初期値ｑ１（０），ｑ２（０），…，ｑｎ（０）、及び制御変数λ０を用いて、反復計算によりＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）を算出する（ステップＳ１２、関節変数算出ステップ）。具体的には、変化量計算部３は、関節変数の初期値に基づいて導出されたロボットＲの先端位置及び先端姿勢と目標とする先端位置及び先端姿勢との差ｆ１，ｆ２，…，ｆｍ＋３を式（２）により算出する。続いて変化量計算部３は、算出されたｆ１，ｆ２，…，ｆｍ＋３に基づいてｆ（ｑｋ）を定義し、そのｆ（ｑｋ）を制御変数の初期値λ０を導入したガウス・ニュートン法に当てはめることで関節変数の変化量Δｑｋを導出する。そして、変化量計算部３は、その変化量Δｑｋと関節変数ベクトルｑｋを式（４）に当てはめることで次の関節変数ベクトルｑｋ＋１を導出する。変化量計算部３は、式（２）〜（４）を用いたこのような処理を評価データ数Ｓの分だけ繰り返す。
【００４９】
続いて評価部４が、導出されたＳ個の関節変数ベクトルｑｋ（ｋ＝１〜Ｓ）に基づいてＳ個の評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）を導出し（ステップＳ１３、制御変数決定ステップ）、評価指標値Ｆ（ｑｋ）の変動状況を取得する（ステップＳ１４、制御変数決定ステップ）。具体的には、評価部４は、Ｓ個の評価指標値Ｆ（ｑｋ）のそれぞれについてＦ（ｑｋ）からＦ（ｑｋ＋１）への変化量ΔＦｋを算出し、変化量が正（負）の値から負（正）の値に切り替わった回数（切替り回数）ｊを取得する。続いて評価部４は、評価データ数Ｓ、制御変数の初期値λ０、及び導出された切替り回数ｊを式（５）に当てはめることで制御変数λを更新する（ステップＳ１５、制御変数決定ステップ）。言い換えれば、評価部４は、評価指標値Ｆ（ｑｋ）の変動状況に基づいて制御変数を決定する。
【００５０】
続いて収束判定部５が、評価指標値Ｆ（ｑＳ）が所定の正数値αよりも小さいか又は関節変数ベクトルの変化量ΔｑＳ−１＝ｑＳ−ｑＳ−１が所定の正数値βよりも小さいか否かを判定する（ステップＳ１６）。ここでＦ（ｑＳ）＜α又はΔｑＳ−１＜βである場合（ステップＳ１６；ＹＥＳ）、収束判定部５は、関節変数ベクトルｑＳが目標値に到達した、すなわちロボットＲの先端の目標位置及び目標姿勢を実現する各関節Ｊ１〜Ｊｎの角度θ１〜θｎが導出されたと判定する。そして、収束判定部５は、関節変数ベクトルｑＳを出力部６に出力する。その後、出力部６が導出された関節変数ベクトルｑＳをロボットＲを動作させる制御部（図示せず）に出力する（ステップＳ１７）。これにより、各関節Ｊ１〜Ｊｎのアクチュエータが作動して各関節の角度θ１〜θｎが変わり、先端Ｔ（ハンドＨ）の位置及び姿勢が目標とするものに変更される。
【００５１】
これに対し、ここでＦ（ｑＳ）≧α且つΔｑＳ−１≧βである場合（ステップＳ１６；ＮＯ）、収束判定部５は、入力された更新後の制御変数λと関節変数ベクトルｑＳとを変化量計算部３に出力する。これにより、今度はｑＳを初期値として、ステップＳ１２以降の処理が実行される。なお、ステップＳ１２における計算では、更新後の制御変数λを導入したガウス・ニュートン法が用いられる。
【００５２】
以上説明したように、本実施形態によれば、制御変数λが、経験的に決められるのではなく評価指標値Ｆ（ｑ）の変動状況に基づいて決定されるので、ロボットの機構に応じて自動的に制御変数λを決定できる。その結果、ロボットの機構に影響されることなく逆運動学計算の解を精度良く導出できる。また、制御変数λを評価指標値Ｆ（ｑ）の変動状況に基づいて決定することで、関節変数ベクトルｑｋの算出を安定化する（関節変数ベクトルｑｋの発散を抑制する）とともに、関節変数ベクトルｑｋが局所解に陥ることを防止することが可能になる。
【００５３】
また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況は、評価データ数Ｓの分だけ算出された評価指標値Ｆ（ｑｋ）（ｋ＝１〜Ｓ）の変化量Ｆｋ（ｋ＝１〜Ｓ−１）に基づいて導出されるので、より適切な制御変数を決定できる。
【００５４】
また、本実施形態によれば、評価指標値Ｆ（ｑ）の変動状況に基づいて決定される制御変数λを導入したガウス・ニュートン法を用いることで（式（３）を用いることで）、算出対象の関節変数ベクトルｑ（関節変数ｑ１〜ｑｎ）が局所解に陥ることを防止し、所望の関節変数ベクトル（関節変数）を算出することが可能になる。
【００５５】
以上、本発明をその実施形態に基づいて詳細に説明した。しかし、本発明は上記実施形態に限定されるものではない。本発明は、その要旨を逸脱しない範囲で以下のような様々な変形が可能である。
【００５６】
本実施形態では、本発明に係る逆運動学計算装置を、ベースＢに固定され多数の関節を有するロボットＲに適用したが、二次元平面上あるいは三次元空間を移動可能なロボットに適用してもよい。
【００５７】
本実施形態では、関節変数ｑ１〜ｑｎが関節Ｊ１〜Ｊｎの関節角度を示したが、関節変数の種類はこれに限定されない。例えば、直動関節により連結されたリンク間の距離を示す関節変数を算出するために、本発明に係る逆運動学計算方法及び逆運動学計算装置を利用できる。
【００５８】
また、本実施形態では、ロボットＲの先端Ｔを作業部位としたが、先端部以外の部分を作業部位としてもよい。例えば、リンク同士を接続する関節を作業部位としてもよいし、人型ロボットの頂部を作業部位としてもよい。
【図面の簡単な説明】
【００５９】
【図１】本実施形態に係る逆運動学計算装置の構成を示す図である。
【図２】図１に示す逆運動学計算装置が適用されるロボットを示す図である。
【図３】（ａ）は、評価指標値Ｆ（ｑ）の変動状況の一例を示すグラフであり、（ｂ）は、評価指標値Ｆ（ｑ）の変動状況の別の例を示すグラフである。
【図４】図１に示す逆運動学計算装置の処理を示すフローチャートである。
【符号の説明】
【００６０】
１…逆運動学計算装置、２…入力部、３…変化量計算部、４…評価部、５…収束判定部、６…出力部
【特許請求の範囲】
【請求項１】
複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、
制御変数を決定する制御変数決定ステップと、
前記制御変数決定ステップにおいて決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップとを含み、
前記制御変数決定ステップでは、前記関節変数算出ステップにおいて算出された関節変数を評価する所定の評価指標値の変動状況に基づいて前記制御変数を決定する、
ことを特徴とする逆運動学計算方法。
【請求項２】
前記関節変数算出ステップでは、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行し、
前記制御変数決定ステップでは、前記評価指標値を算出するための評価関数を用いて、前記関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出する、
ことを特徴とする請求項１に記載の逆運動学計算方法。
【請求項３】
前記関節変数算出ステップでは、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
ことを特徴とする請求項１又は２に記載の逆運動学計算方法。
【請求項４】
複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、
制御変数を決定する制御変数決定手段と、
前記制御変数決定手段により決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段とを備え、
前記制御変数決定手段は、前記関節変数算出手段により算出された関節変数を評価する所定の評価指標値の変動状況に基づいて前記制御変数を決定する、
ことを特徴とする逆運動学計算装置。
【請求項５】
前記関節変数算出手段は、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行し、
前記制御変数決定手段は、前記評価指標値を算出するための評価関数を用いて、前記関節変数算出手段により算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出する、
ことを特徴とする請求項４に記載の逆運動学計算装置。
【請求項６】
前記関節変数算出手段は、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
ことを特徴とする請求項４又は５に記載の逆運動学計算装置。
【請求項１】
複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算方法であって、
制御変数を決定する制御変数決定ステップと、
前記制御変数決定ステップにおいて決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出ステップとを含み、
前記制御変数決定ステップでは、前記関節変数算出ステップにおいて算出された関節変数を評価する所定の評価指標値の変動状況に基づいて前記制御変数を決定する、
ことを特徴とする逆運動学計算方法。
【請求項２】
前記関節変数算出ステップでは、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行し、
前記制御変数決定ステップでは、前記評価指標値を算出するための評価関数を用いて、前記関節変数算出ステップにおいて算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出する、
ことを特徴とする請求項１に記載の逆運動学計算方法。
【請求項３】
前記関節変数算出ステップでは、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
ことを特徴とする請求項１又は２に記載の逆運動学計算方法。
【請求項４】
複数の関節を有するロボットの作業部位の位置及び姿勢から関節変数を計算する逆運動学計算装置であって、
制御変数を決定する制御変数決定手段と、
前記制御変数決定手段により決定された制御変数に基づいて前記関節変数の変化量を算出し、算出された変化量に基づいて前記ロボットの作業部位の位置及び姿勢に対応する関節変数を算出する関節変数算出手段とを備え、
前記制御変数決定手段は、前記関節変数算出手段により算出された関節変数を評価する所定の評価指標値の変動状況に基づいて前記制御変数を決定する、
ことを特徴とする逆運動学計算装置。
【請求項５】
前記関節変数算出手段は、前記関節変数の変化量の算出と前記関節変数の算出とを所定の回数だけ反復して実行し、
前記制御変数決定手段は、前記評価指標値を算出するための評価関数を用いて、前記関節変数算出手段により算出された複数の関節変数のそれぞれについて前記評価指標値を算出し、算出された複数の評価指標値の変化量に基づいて該評価指標値の変動状況を導出する、
ことを特徴とする請求項４に記載の逆運動学計算装置。
【請求項６】
前記関節変数算出手段は、前記制御変数を導入したガウス・ニュートン法を用いて前記関節変数の変化量を算出する、
ことを特徴とする請求項４又は５に記載の逆運動学計算装置。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００９−２３０１６（Ｐ２００９−２３０１６Ａ）
【公開日】平成２１年２月５日（２００９．２．５）
【国際特許分類】
【出願番号】特願２００７−１８５９４５（Ｐ２００７−１８５９４５）
【出願日】平成１９年７月１７日（２００７．７．１７）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２１年２月５日（２００９．２．５）
【国際特許分類】
【出願日】平成１９年７月１７日（２００７．７．１７）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]