(57)【要約】【課題】複数の関節を有する自動機械中の、関節の作動により移動する移動部材の速度に関する物理量を演算するための負荷を減らし、演算処理を速くすることのできる物理量演算方法及び物理量演算装置を提供する。【解決手段】第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）及び第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）で構成される変換行列ｉ−１Ｔｉで、手首Ｘの位置０ｐを示した第１数式を設定した後に、当該第１数式中の変換行列ｉ−１Ｔｉを展開することなく微分することによって、変換行列ｉ−１Ｔｉ及び変換行列ｉ−１Ｔｉを微分した行列で示される第２数式を、変換行列ｉ−１Ｔｉと、第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）を微分した微分行列と、第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）とによって表して物理量を演算する。【選択図】図３
2008272877.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節を有する自動機械における、前記関節の作動により移動する移動部材の速度に関する物理量を演算する物理量演算方法において、
前記関節の回転角度によって定められる変数で示される第１の行列及び前記自動機械の構造によって定められる定数のみで示される第２の行列から構成され、前記複数の関節に含まれる一つの関節に対して定められた座標系を当該一つの関節と隣合う関節に対して定められた座標系に変換する変換行列を設定し、
前記移動部材の位置を前記変換行列で示した第１数式を設定し、
前記第１数式を少なくとも一回以上微分することによって、前記第１の行列を少なくとも一回以上微分した微分行列、前記変換行列、及び前記第２の行列で前記移動部材の前記物理量を示した第２数式を設定し、
前記第２数式に基づいて前記移動部材の前記物理量を演算すること、
を特徴とする物理量演算方法。
【請求項２】
前記第１の行列を少なくとも一回微分することによって得られる前記微分行列を予め設定し、
前記第２数式へ、予め設定した前記微分行列を代入すること、
を特徴とする請求項１記載の物理量演算方法。
【請求項３】
複数の関節を有する自動機械における、前記関節の作動により移動する移動部材の速度に関する物理量を演算する物理量演算装置において、
前記関節の回転角度によって定められる変数で示される第１の行列及び前記自動機械の構造によって定められる定数のみで示される第２の行列から構成され、前記複数の関節に含まれる一つの関節に対して定められた座標系を、当該一つの関節と隣合う関節に対して定められた座標系に変換する変換行列を設定する変換行列設定手段と、
前記移動部材の位置を前記変換行列で示した第１数式を設定する第１数式設定手段と、
前記第１数式を少なくとも一回以上微分することによって、前記第１の行列を少なくとも一回以上微分した微分行列、前記変換行列、及び前記第２の行列で前記移動部材の前記物理量を示した第２数式を設定する第２数式設定手段と、
前記第２数式に基づいて前記移動部材の前記物理量を演算する物理量演算手段と、
を備えることを特徴とする物理量演算装置。
【請求項４】
前記第１の行列を少なくとも一回微分することによって得られる前記微分行列を予め設定し、設定した前記微分行列を前記第２数式へ代入する微分行列設定手段を更に備える微分行列設定部を備えることを特徴とする請求項３記載の物理量演算装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節を有する自動機械における移動部材の速度に関する物理量を演算する物理量演算方法及び物理量演算装置に関する。
【背景技術】
【０００２】
従来、物理量演算方法及び物理量演算装置として、関節の回転角等の関節変数やリンク長等のリンクパラメータによって定められる同次変換行列を設定し、自動機械中の溶接打点のような先端の位置を、それぞれの関節についての複数の同次変換行列によって求めるものが知られている（例えば、特許文献１）。
【特許文献１】特開２００３−１９６８２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、このような方法及び装置においては、溶接打点の速度に関する物理量、すなわち速度、加速度、ジャークを演算する際に、各関節についての同次変換行列を全て計算して展開した形で溶接打点の位置を示した後に演算している。従って、展開して煩雑になった式を微分することによって、速度に関する物理量を演算しなければならないため、演算の負荷が大きく、演算処理に時間がかかるという問題があった。
【０００４】
そこで本発明は、このような技術課題を解決するためになされたものであって、複数の関節を有する自動機械中の、関節の作動により移動する移動部材の速度に関する物理量を演算するための負荷を減らし、演算処理を速くすることのできる物理量演算方法及び物理量演算装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
本発明に係る物理量演算方法は、複数の関節を有する自動機械中の、関節の作動により移動する移動部材の速度に関する物理量を演算する物理量演算方法において、関節の回転角度によって定められる変数で示される第１の行列及び自動機械の構造によって定められる定数のみで示される第２の行列から構成され、複数の関節に含まれる一つの関節に対して定められた座標系を、当該一つの関節と隣合う関節に対して定められた座標系に変換する変換行列を設定し、移動部材の位置を変換行列で示した第１数式を設定し、第１数式を少なくとも一回以上微分することによって、移動部材の物理量を、変換行列、第１の行列を少なくとも一回以上微分した微分行列、及び第２の行列で示した第２数式を設定し、第２数式に基づいて移動部材の物理量を演算すること、を特徴とする。
【０００６】
また、本発明に係る物理量演算装置は、複数の関節を有する自動機械中の、関節の作動により移動する移動部材の速度に関する物理量を演算する物理量演算装置において、関節の回転角度によって定められる変数で示される第１の行列及び自動機械の構造によって定められる定数のみで示される第２の行列から構成され、複数の関節に含まれる一つの関節に対して定められた座標系を、当該一つの関節と隣合う関節に対して定められた座標系に変換する変換行列を設定する変換行列設定手段と、移動部材の位置を変換行列で示した第１数式を設定する第１数式設定手段と、第１数式に基づいて、移動部材の物理量を、変換行列、第１の行列を少なくとも一回以上微分した微分行列、及び第２の行列によって示した第２数式を設定する第２数式設定手段と、第２数式に基づいて移動部材の物理量を演算する物理量演算手段と、を備えることを特徴とする。
【０００７】
この物理量演算方法及び物理量演算装置では、移動部材の位置を変換行列で示した第１数式を設定した後に、当該第１数式中の変換行列を展開することなく微分することによって、物理量を演算する。ここで、変換行列を構成する第２の行列は、自動機械の構造によって定められる定数（リンクパラメータ）のみで示されるため、当該変換行列を微分するときは関節の回転角度によって定められる変数（関数変数）で示される第１の行列のみを微分することとなる。従って、第１数式を一回以上微分することによって変換行列及び変換行列を微分した行列で示される第２数式は、変換行列と、第１の行列を微分した微分行列と、第２の行列とによって示されることとなる。そして、第１の行列を微分する場合は、全ての変換行列を展開して微分した場合に比べて、大幅に演算量を少なくすることができる。これによって、移動部材の速度に関する物理量を演算するための負荷を減らし、演算処理を速くすることができる。
【０００８】
本発明の上記物理量演算方法では、第１の行列を少なくとも一回微分することによって得られる微分行列を予め設定し、第２数式へ、予め設定した微分行列を代入することが好ましい。また、本発明の上記物理量演算装置では、第１の行列を少なくとも一回微分することによって得られる微分行列を予め設定し、設定した微分行列を第２数式へ代入する微分行列設定手段を更に備えることが好ましい。これによれば、前もって設定可能な部分である微分行列の設定のための微分計算を予め行っておき、第２数式へ当該微分行列を代入することによって、物理量を演算する際は、微分の計算を行うことなく物理量を演算することができる。
【発明の効果】
【０００９】
本発明によれば、複数の関節を有する自動機械中の、関節の作動により移動する移動部材の速度に関する物理量を演算するための負荷を減らし、演算処理を速くすることができる。
【発明を実施するための最良の形態】
【００１０】
以下、添付図面を参照して本発明の実施形態について説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００１１】
図１は本発明の実施形態に係る物理量演算方法を実行する物理量演算装置の構成概略図である。図１に示すように、物理量演算装置１は、複数の関節を有する自動機械における、関節の作動により移動する移動部材の速度、加速度、ジャーク等の速度に関する物理量を演算するものであり、自動機械構造入力部２と、関節変数入力部３と、目標位置入力部４と、演算結果出力部６と、演算部７を備えて構成される。なお、物理量演算装置１は、具体的にはコンピュータによって構成されている。
【００１２】
自動機械構造入力部２は、演算の対象となる自動機械のリンクパラメータ（詳細な説明は後述）、リンク数、各関節の回転角度や移動部材の速度のリミット等を入力する部分であり、カメラや各種センサなどの情報取得デバイスや、キーボードやデータを予め格納した光ディスクドライブなどから構成されている。関節変数入力部３は、自動機械の各関節における関節変数（詳細な説明は後述）を入力する部分であり、カメラや各種センサなどの情報取得デバイスなどから構成されている。また、目標位置入力部４は、移動部材の移動目標を入力する部分であり、キーボードやデータを予め格納した光ディスクドライブなどから構成されている。
【００１３】
演算結果出力部６は、例えば自動機械の制御装置に演算部７で演算した結果を送信して、自動機械の制御にフィードバックさせる部分である。
【００１４】
演算部７は、リンクパラメータや関節変数に基づいて移動部材の速度、加速度、ジャーク等の速度に関する物理量を演算するものであり、物理量演算式設定部８と、物理量演算部（物理量演算手段）９と、情報記憶部１１とを備えて構成される。
【００１５】
物理量演算式設定部８は、自動機械の移動部材の速度、加速度、ジャーク等を演算するために用いる物理量演算式を設定する部分であり、変換行列設定部（変換行列設定手段）１２と、第１数式設定部（第１数式設定手段）１３と、第２数式設定部（第２数式設定手段）１４と、微分行列設定部（微分行列設定手段）１６とを備えて構成される。変換行列設定部１２は、自動機械構造入力部２から入力されたリンクパラメータに基づいて、自動機械に含まれる一つの関節に対して定められた座標系を、当該一つの関節と隣合う関節に対して定められた座標系に変換する変換行列を設定する部分である。また、第１数式設定部１３、第２数式設定部１４及び微分行列設定部１６は、移動部材の物理量を効率よく演算するための第１数式、第２数式及び微分行列を設定する部分である（詳細な説明は後述）。
【００１６】
物理量演算部９は、物理量演算式設定部８で設定された物理量演算式と関節変数入力部３から入力された関数変数を用いて、物理量を演算する部分である。このような物理量演算式設定部８及び物理量演算部９は、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。
【００１７】
情報記憶部１１は、自動機械構造入力部２、関節変数入力部３及び目標位置入力部４からの入力情報や物理量演算式設定部８で設定した物理量演算式を記憶すると共に、変換行列設定部１２や物理量演算部９に必要な情報を出力する部分である。このような情報記憶部１１は、具体的にはハードディスクやＲＡＭなどによって実現されている。
【００１８】
本実施形態では、複数の関節を有する自動機械がロボットアームである場合を例にして説明する。図２はロボットアーム１００を模式的に示した図である。このロボットアーム（自動機械）１００は、図２に示すように、ベースＢ上に設けられ、複数のリンクＬ０〜Ｌｎと、複数のジョイントＪ１〜Ｊｎとからなる。なお、リンクＬｉ−１とＬｉの間にジョイントＪｉが設けられるものとする。各ジョイントＪ１〜Ｊｎは一自由度の関節である。また、ロボットアーム１００の先端には、把持部１０３が取り付けられている。ここでは、把持部１０３の根元、すなわち、最も末端側のリンクＬｎの先端部分（手首Ｘ）が目標位置へ移動するまでの間に、物理量演算装置１によって、手首Ｘの速度を繰り返し演算するものとする。この手首Ｘが自動機械における移動部材に相当する。
【００１９】
次に、本実施形態に係る物理量演算方法を実行する物理量演算装置１の処理について説明する。
【００２０】
図３は、本実施形態に係る物理量演算方法の物理量演算式設定処理のフローチャートであり、図４は、本実施形態に係る物理量演算方法の物理量演算処理のフローチャートである。物理量演算式設定処理は、物理量演算装置１をロボットアーム１００に接続した初期段階において、物理量演算式設定部８で実行される。また、物理量演算処理は、ロボットアーム１００の動作時において、物理量演算部９で繰り返し実行される。
【００２１】
図３に示すように、物理量演算式設定処理に際しては、まず、情報記憶部１１からロボットアーム１００のリンクパラメータを取得し（Ｓ１００）、そのリンクパラメータに基づいて変換行列を設定する（Ｓ１０５）。
【００２２】
ここで、図５を参照してリンクパラメータ及び変換行列について詳細に説明する。なお、ロボットの構造を定義する時に、一つの関節によって関係付けられる２つのリンク間の変換行列をもとめるために、Ｄｅｎａｖｉｔ−Ｈａｒｔｅｎｂｅｒｇの定義に基づく方法が用いられる。この方法の詳細については、「ロボット制御の実際」（第１章）  小林尚登  著  社団法人・計測自動制御学会  に記載されている。
【００２３】
まず、ロボットアーム１００の一番目のリンクＬ０のベースＢに対する付け根を原点Ｏ´とし（図２参照）、この原点Ｏ´に基準座標系Σ０が設定される。そして、図５に示すように、ジョイントＪｉに対して、リンクＬｉ−１の方向をｘｉ−１軸、ジョイントＪｉの回転軸方向をｚｉ−１軸、それらと直交する方向をｙｉ−１軸とする座標系Σｉ−１を設定する。そして、ジョイントＪｉの回転軸ｚｉ−１とジョイントＪｉ＋１の回転軸ｚｉに共通法線Ｎｉを引き、この共通法線の長さをリンクＬｉの長さａｉとし、回転軸ｚｉ−１に対する回転軸ｚｉのずれ角度をリンクＬｉのねじれ角αｉとする。一つ手前の共通法線Ｎｉ−１とこの共通法線Ｎｉの回転軸ｚｉ−１上での距離をリンク間のオフセットｄｉとし、回転軸ｚｉ−１に垂直な面内で反時計回りに図った共通法線Ｎｉ，Ｎｉ−１間の角度をリンクの回転角度θｉとする。ここで、長さａｉ、ねじれ角αｉ及びオフセットｄｉは、ロボットアーム１００の構造によって一義的に決まる値であり、リンクパラメータと定義する。回転角度θｉは、ロボットアーム１００の動きによって変動する値であり、関節変数と定義する。定数であるリンクパラメータは初期段階において、自動機械構造入力部２から一度入力されるのみの値であるが、変数である関節関数は、ロボットアーム１００の動作中、関節変数入力部から常に入力され続ける値である。
【００２４】
上述のようなリンクパラメータと関節変数を考慮すると、座標系Σｉ−１を回転軸ｚｉ−１回りに回転角度θｉだけ回転させ、回転軸ｚｉ−１に沿ってオフセットｄｉだけ並進させ、回転後のｘｉ−１軸すなわちｘｉ軸に沿って長さａｉだけ並進させ、ｘｉ軸回りにねじれ角αｉだけ回転させることによって座標系Σｉが表現される。以上のことから、座標系Σｉ−１から座標系Σｉへ変換する変換行列ｉ−１Ｔｉを（１）のように表すことができる。以上によって、物理量演算式設定処理に際しては、情報記憶部１１から取得したリンクパラメータによって各座標系についての変換行列ｉ−１Ｔｉが設定される。
【００２５】
【数１】
2008272877.tif 000003
【００２６】
図３に戻り、変換行列ｉ−１Ｔｉによって、（２）に示すように、座標系Σｎからみたときの手首Ｘの位置ｎｐと座標系Σ０からみたときの手首Ｘの位置０ｐとの関係を表す第１数式を設定する（Ｓ１１０）。更に、この第１数式を微分することによって、（３）に示すように、手首Ｘの位置を微分した値、すなわち手首Ｘの速度を表す第２数式を設定する（Ｓ１１５）。
【００２７】
【数２】
2008272877.tif 000004
【００２８】
【数３】
2008272877.tif 000005
【００２９】
ここで、（１）から分かるように、変換行列ｉ−１Ｔｉは、（４）に示すように、関節変数である回転角度θｉのみによって示される第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）と、（５）に示すように、定数であるリンクパラメータのみによって示される第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）から構成されている。
【００３０】
【数４】
2008272877.tif 000006
【００３１】
【数５】
2008272877.tif 000007
【００３２】
従って、変換行列ｉ−１Ｔｉを微分した行列は、（６）に示すように、第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）のみを微分することによって表される。ここで、微分行列設定部１６で第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）を微分することによって、（７）のような微分行列を設定する（Ｓ１２０）。
【００３３】
【数６】
2008272877.tif 000008
【００３４】
【数７】
2008272877.tif 000009
【００３５】
以上より、（３）で示した第２数式へ（１）に示す変換行列ｉ−１Ｔｉと（６）に示す変換行列ｉ−１Ｔｉを微分した行列を代入すると、第２数式の右辺は、微分行列と、第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）と、変換行列ｉ−１Ｔｉと、座標系Σｎからみたときの手首Ｘの位置ｎｐとから表されることとなる。そして、このように表された第２数式へ、（７）に示す予め設定した微分行列を代入する（Ｓ１２５）ことによって物理量演算式を設定する（Ｓ１３０）。この物理量演算式は、微分の計算を全く含まない行列のみで示され、回転角度θ１〜θｎと座標系Σｎからみたときの手首Ｘの位置ｎｐの値のみを代入して計算するだけで手首Ｘの速度を演算することができる。物理量演算式を設定したら、情報記憶部１１に格納する。
【００３６】
次に、図４を参照して物理量演算処理について説明する。
【００３７】
図４に示すように、物理量演算処理に際しては、まず情報記憶部１１から上述のように設定した物理量演算式を取得する（Ｓ１５０）。その後、情報記憶部１１に格納されている関節変数である回転角度θ１〜θｎを取得する（Ｓ１５５）と共に、回転角度θｎ及びリンクパラメータに基づいて、座標系Σｎからみたときの手首Ｘの位置ｎｐの値を演算する（Ｓ１６０）。そして、物理量演算式へ回転角度θ１〜θｎと手首Ｘの位置ｎｐを代入し（Ｓ１６５）、行列を展開して計算する（Ｓ１７０）ことによって手首Ｘの速度を算出する。なお、物理量演算装置１で演算した速度結果が、予め定められた速度のリミット値を超えているときなどは、各ジョイントＪの回転速度を遅くさせるなどして手首Ｘの速度を制御する。
【００３８】
以上のように、本実施形態に係る物量演算方法によれば、手首Ｘの位置０ｐを変換行列ｉ−１Ｔｉで示した第１数式を設定した後に、当該第１数式中の変換行列ｉ−１Ｔｉを展開することなく微分することによって、速度を演算する。ここで、変換行列ｉ−１Ｔｉを構成する第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）は、ロボットアーム１００の構造によって定められる定数（リンクパラメータ）のみで示されるため、当該変換行列ｉ−１Ｔｉを微分するときはジョイントＪｉの回転角度θｉによって定められる変数（関数変数）で示される第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）のみを微分することとなる。従って、第１数式を微分することによって変換行列ｉ−１Ｔｉ及び変換行列ｉ−１Ｔｉを微分した行列で示される第２数式は、第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）を微分した微分行列と、変換行列ｉ−１Ｔｉと、第２の行列Ｔｔｒａｎｓ（ａｉ，０，ｄｉ）・Ｔｒｏｔ（ｘｉ，αｉ）とによって示されることとなる。そして、第１の行列Ｔｒｏｔ（ｚｉ−１，θｉ）を微分する場合は、全ての変換行列ｉ−１Ｔｉを展開して微分した場合に比べて、大幅に演算量を少なくすることができる。これによって、手首Ｘの速度を演算するための負荷を減らし、演算処理を速くすることができる。
【００３９】
また、前もって演算可能な部分である微分行列の部分の演算を予め行っておき第２数式に当該微分行列を代入して物理量演算式を設定しているため、速度を演算する際は、微分の計算を行うことなく速度を演算することができる。
【００４０】
本発明は、上述した実施形態に限定されるものではない。
【００４１】
例えば、手首Ｘの速度を演算しているが、各ジョイントＪの速度を演算してもよい。これによって、各ジョイントＪの速度を見ながら制御することによって、ロボットアーム１００全体として効率よく動作させることができるようになる。
【００４２】
また、速度を演算しているが、位置を２回微分することによって加速度を、３回微分することによってジャークを、あるいはそれ以上微分した物理量を演算してもよい。このような場合も、第１の行列を予め複数回微分した微分行列を設定し、第１数式を複数回微分することによって表した数式に微分行列を代入することによって、負荷が低減された速い演算処理を行うことができる。
【００４３】
更に、ロボットアーム１００のジョイントＪは、回転するものに限らず、直進するものであってもよい。
【図面の簡単な説明】
【００４４】
【図１】本発明の実施形態に係る物理量演算方法を実行する物理量演算装置の構成概略図である。
【図２】物理量演算の対象となるロボットアームを模式的に示した図である。
【図３】本実施形態に係る物理量演算方法の、物理量演算式設定処理のフローチャートである。
【図４】本実施形態に係る物理量演算方法の、物理量演算処理のフローチャートである。
【図５】ロボットアームのジョイントとリンクの関係を示した図である。
【符号の説明】
【００４５】
１…物理量演算装置、９…物理量演算部（物理量演算手段）、１２…変換行列設定部（変換行列設定手段）、１３…第１数式設定部（第１数式設定手段）、１４…第２数式設定部（第２数式設定手段）、１６…微分行列設定部（微分行列設定手段）、１００…ロボットアーム（自動機械）、Ｊ…関節、Ｘ…手首（移動部材）。
【図１】
2008272877.tif 000010
【図２】
2008272877.tif 000011
【図３】
2008272877.tif 000012
【図４】
2008272877.tif 000013
【図５】
2008272877.tif 000014