JP2017061022A - ロボットの制御装置 - Google Patents

Abstract

【課題】修正ＤＨパラメータに基づいてロボットの各回転部の角度を算出できるロボットの制御装置を提供する。【解決手段】ロボットのアーム先端部を制御点とする。コントローラは、制御点の目標となる位置及び姿勢を仮の位置及び姿勢として設定し、設定した仮の位置及び姿勢をＤＨパラメータに基づいて逆変換処理することにより、各回転部の仮の角度Ｊｔｅｍｐを算出する。コントローラは、算出した各回転部の仮の角度Ｊｔｅｍｐを修正ＤＨパラメータに基づいて順変換処理することにより、制御点の位置及び姿勢を算出し、算出した位置及び姿勢と目標となる位置及び姿勢とのずれ量を算出する。コントローラは、算出したずれ量が閾値よりも小さくなるまで仮の位置及び姿勢の更新を繰り返し、算出したずれ量が閾値よりも小さくなった場合に、逆変換処理により算出されている仮の角度Ｊｔｅｍｐを各回転部の角度Ｊｒｅｆとして算出する。【選択図】 図３

Description

本発明は、複数の回転部と、複数の前記回転部を順次連結してかつ隣り合う前記回転部を互いに回転可能にする関節と、を含むアームを備えるロボットに適用される制御装置に関する。

この種の制御装置としては、下記特許文献１に見られるように、６軸の垂直多関節型のロボットの動作に伴い各関節及び各回転部（リンク）に発生するたわみに起因して、ロボットの制御点である手先の位置及び姿勢が目標となる位置及び姿勢からずれることを抑制するものが知られている。この制御装置では、位置及び姿勢のずれを抑制するための処理として、修正ＤＨパラメータに基づく順変換処理と、修正ＤＨパラメータに基づく逆変換処理とを用いている。

修正ＤＨパラメータとは、ロボットのリンク機構のモデルを表すパラメータであり、ＤＨパラメータとは異なるパラメータである。詳しくは、修正ＤＨパラメータとは、各関節に対して３軸直交座標系を規定した場合において隣り合う関節に対応する３軸直交座標系の関係を表すパラメータである。下記特許文献１では、修正ＤＨパラメータとして、互いに直交する３軸のそれぞれを回転中心軸線とした回転角度量と、互いに直交する２軸方向のそれぞれにおける平行移動量とを用いている。

下記特許文献１には、以下のことが記載されている。詳しくは、まず、目標となる位置及び姿勢ｘｓｔが仮の位置及び姿勢ｘとして設定される。そして、設定された仮の位置及び姿勢ｘが修正ＤＨパラメータに基づいて逆変換処理されることにより、ロボットの各回転部の角度θｄが算出される。続いて、算出された各回転部の角度θｄが修正ＤＨパラメータに基づいて順変換処理されることにより、手先の位置及び姿勢ｘｄが算出される。

特許第３８０８３２１号公報

上記特許文献１には、修正ＤＨパラメータに基づく逆変換処理により角度を算出すると記載されているものの、実際には、修正ＤＨパラメータに基づく逆変換処理により角度を算出することを保証することはできない。

つまり、通常、逆変換処理は解析的に行われる。逆変換処理を解析的に行うためには、ロボットにおいてひと続きの３つの関節の回転軸線が１点で交わるとの交差条件が成立する必要がある。ここで、多関節を有するロボットにおいては、交差条件が成立するように設計されているため、ＤＨパラメータに基づく逆変換処理を解析的に行うことができる。例えば図４に示すように、６つ関節を有するロボットにおいては、アームの根元部から数えて、第４，５，６番目の関節の回転軸線Ｊ４，Ｊ５，Ｊ６が１点で交わるため、ＤＨパラメータに基づく逆変換処理を解析的に行うことができる。

しかしながら、修正ＤＨパラメータを用いてロボット組み付け時の誤差をロボットリンク機構に反映させると、上記交差条件が成立しなくなる。このため、修正ＤＨパラメータを用いる場合には、逆変換処理を解析的に行うことができなくなり、各回転部の角度を算出することができなくなる。

そこで、ヤコビ行列を用いた繰り返し演算により修正ＤＨパラメータに基づく逆変換処理を行うことも考えられる。しかしながら、ロボットの特異点近傍では、ヤコビ行列の行列式が非常に小さな値となることによりヤコビ行列の逆行列が大きな値となり、正しい角度を算出することができなくなる。また、ロボットの特異点では、ヤコビ行列の行列式が０となり、ヤコビ行列の逆行列を算出できなくなる。したがって、特異点及びその近傍においては、ヤコビ行列を用いた繰り返し演算によっても、修正ＤＨパラメータに基づく逆変換処理による角度の算出を保証することはできない。

本発明は、上述した問題を解決するためになされたものであり、修正ＤＨパラメータに基づいてロボットの各回転部の角度を算出できるロボットの制御装置を提供することを主たる目的とするものである。

第１の発明は、複数の回転部と、複数の前記回転部を順次連結してかつ隣り合う前記回転部を互いに回転可能にする関節と、を含むアームを備えるロボットに適用され、前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を仮の位置及び姿勢として設定する仮設定手段と、前記各関節の回転中心軸線がＺ軸とされ、隣り合う前記関節に対応する前記Ｚ軸の共通垂線に沿って前記Ｚ軸から延びる軸線がＸ軸とされ、隣り合う前記関節の前記Ｘ軸方向における距離、隣り合う前記関節に対応する前記Ｚ軸が前記Ｘ軸まわりになす角度、隣り合う前記関節の前記Ｚ軸方向における距離、及び隣り合う前記関節に対応する前記Ｘ軸を前記Ｚ軸まわりになす角度がＤＨパラメータとして定義されており、前記仮設定手段により設定された前記仮の位置及び姿勢を前記ＤＨパラメータに基づいて逆変換処理することにより、前記各回転部の仮の角度を算出する逆変換手段と、前記各関節に対して３軸直交座標系が規定され、隣り合う前記関節に対応する前記３軸直交座標系の関係を表すパラメータが、互いに直交する３軸のそれぞれを回転中心軸線とした回転角度量と、互いに直交する３軸のうち少なくとも２軸方向のそれぞれにおける平行移動量とを含む修正ＤＨパラメータとして定義されており、前記逆変換手段により算出された前記各回転部の仮の角度を前記修正ＤＨパラメータに基づいて順変換処理することにより、前記制御点の位置及び姿勢を算出する順変換手段と、前記順変換手段により算出された位置及び姿勢と前記目標となる位置及び姿勢とのずれ量を算出するずれ量算出手段と、前記ずれ量算出手段により算出されたずれ量が閾値よりも小さくなるまで、前記仮の位置及び姿勢の更新を繰り返す更新手段と、前記ずれ量算出手段により算出されたずれ量が前記閾値よりも小さくなった場合に、前記逆変換手段により算出されている前記各回転部の仮の角度を、前記各回転部の角度として算出する角度算出手段と、を備えることを特徴とする。

上記発明では、各関節の回転中心軸線がＺ軸とされ、隣り合う関節に対応するＺ軸の共通垂線に沿ってＺ軸から延びる軸線がＸ軸とされている。Ｘ，Ｚ軸を用いて、ロボットのリンク機構を表す４つのパラメータからなるＤＨパラメータが定義されている。詳しくは、ＤＨパラメータは、隣り合う関節のＸ軸方向における距離、隣り合う関節に対応するＺ軸がＸ軸まわりになす角度、隣り合う関節のＺ軸方向における距離、及び隣り合う関節に対応するＸ軸がＺ軸まわりになす角度からなる。

また上記発明では、少なくとも５つのパラメータからなる修正ＤＨパラメータが定義されている。修正ＤＨパラメータは、隣り合う関節に対応する３軸直交座標系の関係を表すパラメータであり、ロボット機構の誤差等を補償して位置及び姿勢の算出精度を高めるために採用される。詳しくは、修正ＤＨパラメータは、互いに直交する３軸のそれぞれを回転中心軸線とした回転角度量と、互いに直交する３軸のうち少なくとも２軸方向のそれぞれにおける平行移動量とを含むパラメータである。

上記発明では、まず、仮設定手段により、制御点の目標となる位置及び姿勢が仮の位置及び姿勢として設定される。そして、ＤＨパラメータに基づいて解析的に逆変換処理が可能なことに鑑み、逆変換手段により、ＤＨパラメータに基づいて仮の位置及び姿勢が逆変換処理されることで各回転部の仮の角度が算出される。

そして、順変換手段により、算出された仮の角度が修正ＤＨパラメータに基づいて順変換処理されることで制御点の位置及び姿勢が算出される。ＤＨパラメータにより表現されるリンク機構と修正ＤＨパラメータにより表現されるリンク機構とは異なるため、順変換手段により算出された制御点の位置及び姿勢と、目標となる位置及び姿勢との間にはずれが生じることとなる。

そこで、ずれ量算出手段により、順変換手段により算出された制御点の位置及び姿勢と目標となる位置及び姿勢とのずれ量が算出され、算出されたずれ量が閾値よりも小さくなるように仮の位置及び姿勢が更新される。そして、逆変換手段により、更新された仮の位置及び姿勢がＤＨパラメータに基づいて再度逆変換処理されることで各回転部の仮の角度が算出される。そして、順変換手段により、算出された仮の角度が修正ＤＨパラメータに基づいて再度順変換処理されることで制御点の位置及び姿勢が算出される。そして、ずれ量算出手段により、順変換手段により算出された制御点の位置及び姿勢と目標となる位置及び姿勢とのずれ量が再度算出される。ここで、再度算出されたずれ量が閾値よりも小さくなることは、逆変換処理により既に算出されている仮の角度と、目標となる位置及び姿勢が修正ＤＨパラメータに基づいて逆変換されることにより導かれた角度とが実質的に等しくなることを裏付けるものである。

この点に鑑み、ずれ量算出手段により算出されたずれ量が閾値よりも小さくなるまで、逆変換手段で用いられる仮の位置及び姿勢の更新が繰り返される。そして、ずれ量算出手段により算出されたずれ量が閾値よりも小さくなった場合に、逆変換手段により既に算出されている各回転部の仮の角度が、各回転部の角度として算出される。

このように上記発明は、ＤＨパラメータに基づく逆変換処理の実行後に修正ＤＨパラメータに基づく順変換処理を実行することにより算出された位置及び姿勢と、目標となる位置及び姿勢とが一致するか否かの裏取りを行うものである。そして上記発明は、位置及び姿勢が一致した場合においてＤＨパラメータに基づく逆変換処理により既に算出されている角度を、修正ＤＨパラメータに基づく逆変換処理により算出された角度と同じものとして扱うことができる。したがって上記発明によれば、目標となる位置及び姿勢に修正ＤＨパラメータに基づく逆変換処理を直接施すことなく、目標となる位置及び姿勢に対応する各回転部の角度を修正ＤＨパラメータに基づいて算出することができる。

ここで、角度算出手段により算出された各回転部の角度は、ロボットに各回転部のそれぞれを駆動するサーボモータが備えられる構成において以下のように用いることができる。詳しくは、第２の発明では、前記角度算出手段は、動作周期毎に前記各回転部の角度を算出し、前記各回転部の現在の角度から、前記角度算出手段により算出された前記各回転部の角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動手段を備えることを特徴とする。

第３の発明は、前記修正ＤＨパラメータとして、前記ロボットを測定して得られた測定値を反映した値が設定されることを特徴とする。

上記発明では、ロボットの個体差に起因したロボット機構の誤差等を修正ＤＨパラメータに反映することができる。このため、制御点の位置及び姿勢の算出精度を高めることができる。

ロボット、コントローラ、及びティーチングペンダントの概要を示す図。 ロボットの各関節に規定された座標系を示す図。 修正ＤＨパラメータに基づく各回転部の角度算出処理の手順を示すフローチャート。 ６軸ロボットアームのモデルを示す図。

以下、垂直多関節型ロボットの制御装置に具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。

はじめに、ロボット１０の概要を図１に基づいて説明する。

図示されるように、ロボット１０は、第１〜第６回転部（リンク）を順次連結する各関節の回転中心軸線として、第１〜第６軸線を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、サーボモータの駆動により原点位置を基準として各回転部が動作する。各サーボモータには、その出力軸を制動する電磁ブレーキと、出力軸の角度に応じたパルス信号を出力するエンコーダとがそれぞれ設けられている。

ロボット１０は、床等の据え付け面に設置されており、第１軸線Ｊ１が鉛直方向へ延びている。ロボット１０において、基台１１は、据え付け面に固定される固定部１２と、その固定部１２の上方に設けられる第１回転部１３（ショルダ部）とを有している。ロボット１０のアームは、第１回転部１３に加え、第２回転部１４（下アーム部）、第３，第４回転部１５Ａ，１５Ｂ（上アーム部１５）、第５回転部１６（手首部）、及び第６回転部１７（ハンド部）を備えている。

第１回転部１３は、アームの両端部のうち、アーム先端部とは反対側の根元部に相当する。第１回転部１３は、第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、第１回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。

第２回転部１４が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、第２回転部１４は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、第１回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。第２回転部１４は、基本姿勢として鉛直方向に延びる向きに設けられている。

第２回転部１４の上端部には、上アーム部１５が、水平方向に延びる第３軸線Ｊ３を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム部１５は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、第２回転部１４により第３軸線Ｊ３を中心として回転可能に支持されている。上アーム部１５は、基本姿勢として水平方向に延びる向きに設けられている。

上アーム部１５は、基端側（回転の際に第３軸線Ｊ３を回転中心とする関節側）と先端側とで２つのアーム部に分割されて構成されており、基端側は第３回転部１５Ａ（第１上アーム部）、先端側は第４回転部１５Ｂ（第２上アーム部）となっている。第４回転部１５Ｂは、上アーム部１５の長手方向に延びる第４軸線Ｊ４を回転中心として、第３回転部１５Ａに対してねじり方向に回転可能になっている。すなわち、第４回転部１５Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第３回転部１５Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。

第４回転部１５Ｂの先端部には、第５回転部１６が設けられている。第５回転部１６は、水平方向に延びる第５軸線Ｊ５を回転中心として、第４回転部１５Ｂに対して回転可能になっている。すなわち、第５回転部１６は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第４回転部１５Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。

第５回転部１６の先端部には、ワークやツール等を取り付けるための第６回転部１７が設けられている。第６回転部１７は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、第６回転部１７は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、第５回転部１６により第６軸線Ｊ６を中心として回転可能に支持されている。

コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの角度を検出する。

ティーチングペンダント４０（操作機）は、ＣＰＵ、ＲＯＭ、及びＲＡＭを含むマイクロコンピュータ、各種の手動操作キー、並びにディスプレイ４２等を備えている。ペンダント４０は、コントローラ３０に接続されており、コントローラ３０と通信可能となっている。オペレータ（使用者）は、このペンダント４０を手動操作して、ロボット１０の動作プログラムの作成、修正、登録、各種パラメータの設定を行うことができる。動作プログラムの修正等を行うティーチングでは、作業においてアーム先端部の制御点が通過する教示点を教示する。そして、オペレータは、コントローラ３０を通じて、ティーチングされた動作プログラムに基づきロボット１０を動作させることができる。換言すれば、コントローラ３０は、予め設定された動作プログラム及びペンダント４０の操作に基づいて、ロボット１０のアームの動作を制御する。なお本実施形態では、制御点として、第６回転部１７の中心点１７ａであるＴＣＰ（Tool Center Point）が設定されている。

コントローラ３０は、予め設定された動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置及び姿勢を制御する。本実施形態において、コントローラ３０は、ＣＰ（Continuous Path）制御を行う。ＣＰ制御では、アーム先端部の制御点を目標まで動作させる際に制御点の目標となる位置及び姿勢（動作軌道）が時間関数として設定される。目標となる位置及び姿勢には、教示された位置及び姿勢に加えて、教示された位置及び姿勢に基づいて補間された位置及び姿勢も含まれる。コントローラ３０は、ＣＰ制御により、制御点の位置及び姿勢が動作軌道に沿うように、アームにおける各関節の角度を制御する。

コントローラ３０は、位置及び姿勢の制御において、現在指示されている目標となる位置及び姿勢を実現するための第１〜第６回転部１３〜１７の角度を算出する。本実施形態では、ＤＨパラメータに基づく逆変換処理と、修正ＤＨパラメータに基づく順変換処理とを交互に繰り返すことにより、第１〜第６回転部１３〜１７の角度を算出する手法を採用する。この手法を採用したのは、以下に説明する理由のためである。

通常、逆変換処理は解析的に行われる。逆変換処理を解析的に行うためには、ロボット１０においてひと続きの３つの関節の回転軸線が１点で交わるとの交差条件が成立する必要がある。ここで本実施形態に係るロボット１０では、第４，５，６軸線Ｊ４，Ｊ５，Ｊ６が１点で交わるため、ＤＨパラメータに基づく逆変換処理を解析的に行うことができる。しかしながら、ロボット１０の組み付け時の誤差を修正ＤＨパラメータを用いてロボットリンク機構に反映させると、上記交差条件が成立しなくなる。このため、修正ＤＨパラメータを用いる場合には、逆変換処理を解析的に行うことができなくなり、第１〜第６回転部１３〜１７の角度を算出することができなくなる。

そこで、ヤコビ行列を用いた繰り返し演算により修正ＤＨパラメータに基づく逆変換処理を行うことも考えられる。具体的には、まず、各回転部１３〜１７の角度θｔｍの初期値を設定し、設定した角度θｔｍを順変換処理することにより位置及び姿勢Ｐｔｍを算出するとともに、設定した角度θｔｍに対応するヤコビ行列Ｊｔｍを算出する。そして、算出した位置及び姿勢Ｐｔｍと目標となる位置及び姿勢Ｐｒｅｆとの偏差ΔＰｔｍを算出し、算出した偏差ΔＰｔｍと、ヤコビ行列Ｊｔｍの逆行列とに基づいて、下式（ｅｑ１）のように上記角度θｔｍを更新する。

上述した角度θｔｍの順変換処理から角度θｔｍを更新する処理までの一連の処理は、偏差ΔＰｔｍが十分小さくなるまで繰り返される。

ここで、ロボット１０の特異点近傍では、ヤコビ行列Ｊｔｍの行列式が非常に小さな値となることによりヤコビ行列Ｊｔｍの逆行列が大きな値となり、上式（ｅｑ１）の「Ｊｔｍ＾（−１）ΔＰｔｍ」の値が非常に大きくなる。その結果、角度θｔｍが発散し、正しい角度を算出することができなくなる。また、ロボット１０の特異点では、ヤコビ行列Ｊｔｍの行列式が０となり、ヤコビ行列Ｊｔｍの逆行列を算出することができない。したがって、特異点及びその近傍においては、ヤコビ行列を用いた繰り返し演算によっても、修正ＤＨパラメータに基づく逆変換処理による第１〜第６回転部１３〜１７の角度の算出を保証することはできない。

また、６軸のロボット１０におけるヤコビ行列Ｊｔｍは６×６の行列となり、ヤコビ行列Ｊｔｍの逆行列を算出する場合には演算量が大きくなる。このため、ヤコビ行列Ｊｔｍを用いた繰り返し演算を行うと、繰り返し演算毎に逆行列を算出する必要があることから、演算量が膨大なものとなる。

上述した問題を解決するために、本実施形態では、ＤＨパラメータに基づく逆変換処理と、修正ＤＨパラメータに基づく順変換処理とを組み合わせた第１〜第６回転部１３〜１７の角度算出手法を採用する。以下、この角度算出手法について説明する。

まず、角度算出手法の説明に先立ち、図２を用いて、各関節に規定される座標系について説明する。

図２に示すように、ロボットの各関節には、３次元の直交座標系である第１〜第６座標系Ｒ１〜Ｒ６が規定されている。第１〜第６座標系Ｒ１〜Ｒ６の原点Ｏ１〜Ｏ６は、第１〜第６軸線Ｊ１〜Ｊ６上の所定位置に定められている。第１〜第６座標系Ｒ１〜Ｒ６のＺ軸であるＺ１〜Ｚ６軸は、第１〜第６軸線Ｊ１〜Ｊ６と一致している。第１〜第６座標系Ｒ１〜Ｒ６は、右手系にて規定されている。

固定部１２には、ロボット座標系である第０座標系Ｒ０が規定されている。第０座標系Ｒ０は、第１〜第６回転部１３〜１７が回転しても変化しない座標系である。本実施形態において、第０座標系Ｒ０の原点Ｏ０は、第１軸線Ｊ１上に定められている。また、第０座標系Ｒ０のＺ軸であるＺ０軸は、第１軸線Ｊ１に一致している。

第６回転部１７には、手先座標系である第７座標系Ｒ７が規定されている。本実施形態において、第７座標系Ｒ７の原点Ｏ７は、第６軸線Ｊ６上に定められている。

続いて、第０〜第７座標系Ｒ０〜Ｒ７の相互関係を規定するＤＨパラメータ及び修正ＤＨパラメータについて説明する。

まず、ＤＨパラメータについて説明する。固定部１２をリンク０（ｉ＝０）とし、固定部１２側から第６回転部１７に向かって関節に番号ｉ＝１〜６を付与する。ここで、第ｉ関節の回転中心軸線は、第ｉ軸線Ｊｉに一致する。ここで、固定部１２における第０座標系Ｒ０と第７座標系Ｒ７との関係を規定する同次変換行列は下式（ｅｑ２）で表わされる。

上式（ｅｑ２）の右辺における各行列は、４×４の行列であり、下式（ｅｑ３），（ｅｑ４）で表される。

上式（ｅｑ４）において、ＴｔｘｉはＸ軸平行移動行列であり、ＴｔｚｉはＺ軸平行移動行列である。Ｘ軸平行移動行列Ｔｔｘｉにおいて、Ａｉは、第ｉ−１関節のＺｉ−１軸と第ｉ関節のＺｉ軸との共通垂線の長さ（リンク長さ）であるＸ軸平行移動量を示す。Ｚ軸平行移動行列Ｔｔｚｉにおいて、Ｓｉは、第ｉ−１関節のＸｉ−１軸と第ｉ関節のＸｉ軸との共通垂線の長さであるＺ軸平行移動量を示す。また、上式（ｅｑ４）において、ＴｒｘｉはＸ軸回転行列であり、ＴｒｚｉはＺ軸回転行列である。Ｘ軸回転行列Ｔｒｘｉにおいて、αｉは、Ｘｉ−１軸まわりのＺｉ−１軸とＺｉ軸とのなす角度を示す。Ｚ軸回転行列Ｔｒｚｉにおいて、θｉは、Ｚｉ軸まわりのＸｉ−１軸とＸｉ軸とのなす角度である関節角を示す。なお、上式（ｅｑ２）におけるＸｔは、第６回転部１７における第６座標系Ｒ６から第７座標系Ｒ７への同次変換行列を示す。

なお、図２には、Ｚ１軸とＺ２軸との間のＸ軸平行移動量Ａ２と、Ｘ１軸とＸ２軸との間のＺ軸平行移動量Ｓ２とを例示した。

続いて、修正ＤＨパラメータについて説明する。固定部１２における第０座標系Ｒ０と第７座標系Ｒ７との関係を規定する同次変換行列は下式（ｅｑ５）で表わされる。

上式（ｅｑ５）の右辺における各行列は、下式（ｅｑ６），（ｅｑ７）で表される。

上式（ｅｑ７）において、Ｔγｉは、静止状態のロボット１０の第ｉ関節の初期角度を表す回転行列である。この回転行列Ｔγｉの角度γは、例えばロボット１０の製造工程においてキャリブレーションにより予め設定される。Ｋｒｚｉは第１回転行列であり、第１回転行列Ｋｒｚｉにおけるθｉは、Ｚｉ−１軸まわりの第ｉ−１座標系の回転角度量を示す。Ｋｔｉは平行移動行列であり、平行移動行列Ｋｔｉにおけるａｉ，ｂｉ，ｄｉは、Ｚｉ−１軸まわりにθｉ回転させた第ｉ−１座標系のＸｉ−１軸方向，Ｙｉ−１軸方向，Ｚｉ−１軸方向における平行移動量を示す。

Ｋｒｘｉは第２回転行列である。第２回転行列Ｋｒｘｉにおけるαｉは、Ｚｉ−１軸まわりにθｉ回転させられた後にａｉ，ｂｉ，ｄｉ平行移動させられた第ｉ−１座標系のＸｉ−１軸まわりの回転角度量を示す。Ｋｒｙｉは第３回転行列である。第３回転行列Ｋｒｙｉにおけるβｉは、Ｚｉ−１軸まわりにθｉ回転させられた後にａｉ，ｂｉ，ｄｉ平行移動させられ、その後Ｘｉ−１軸まわりにαｉ回転させられた第ｉ−１座標系のＹｉ−１軸まわりの回転角度量を示す。

角度γｉに加え、修正ＤＨパラメータαｉ，βｉ，θｉ，ａｉ，ｂｉ，ｄｉに量産される各ロボットに応じた値を設定することにより、ロボットのリンク機構の組み付け時における誤差や、ロボットを据え付けた場合の誤差等を補償することができる。すなわち、修正ＤＨパラメータαｉ，βｉ，θｉ，ａｉ，ｂｉ，ｄｉとして、各ロボットを測定して得られた測定値を反映した値を設定する。

次に、図３を用いて、第１〜第６回転部１３〜１７の角度算出処理について説明する。図３は、この処理の手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期（制御周期）毎に繰り返し実行される。動作周期Ｔｒは、例えば８ｍｓである。

この一連の処理では、まずステップＳ１０において、カウンタＮｒを０とする。

続くステップＳ１１では、目標となる位置及び姿勢を表す４×４の目標行列Ｔｒｅｆを取得する。本実施形態において、目標行列Ｔｒｅｆは、位置及び姿勢がロボット座標系である第０座標系Ｒ０を基準として表されている。目標行列Ｔｒｅｆでは、この行列の左上の３×３の行列が制御点の姿勢を表す姿勢行列となっており、右上の３×１の行列が制御点の位置（原点Ｏ０からの位置ベクトル）を表す行列となっている。そしてステップＳ１１では、取得した目標行列Ｔｒｅｆを仮の位置及び姿勢を表す仮行列Ｔｎｅｗとして設定する。

続くステップＳ１２では、ＤＨパラメータに基づいて仮行列Ｔｎｅｗを逆変換処理することにより、第１〜第６回転部１３〜１７の仮の角度Ｊｔｅｍｐを算出する。具体的には、上式（ｅｑ２）に基づいて、第１〜第６回転部１３〜１７の仮の角度Ｊｔｅｍｐを算出する。

続くステップＳ１３では、修正ＤＨパラメータに基づいて第１〜第６回転部１３〜１７の仮の角度Ｊｔｅｍｐを順変換処理することにより、制御点の位置及び姿勢を表す行列Ｔｔｅｍｐを算出する。具体的には、上式（ｅｑ５）に基づいて、行列Ｔｔｅｍｐを算出する。

続くステップＳ１４では、ステップＳ１３で算出した行列Ｔｔｅｍｐから定まる制御点の位置Ｐｔｅｍｐから、目標行列Ｔｒｅｆから定まる制御点の位置Ｐｒｅｆを減算することにより、位置ずれ量ΔＰを算出する。

続くステップＳ１５では、ステップＳ１３で算出した行列Ｔｔｅｍｐから定まる制御点の姿勢と、目標行列Ｔｒｅｆから定まる制御点の姿勢とのずれ量である姿勢ずれ量ΔＡを算出する。以下、本実施形態に係る姿勢ずれ量ΔＡの算出手法を説明する。

姿勢ずれ量ΔＡの算出には、回転中心軸線となる単位ベクトルＶと、姿勢ずれ量としての角度回転量ΔＡとによって姿勢行列Ｒを表現するオイラーベクトルが用いられる。ここで、姿勢行列Ｒ、単位ベクトルＶ及び角度回転量ΔＡの関係は、下式（ｅｑ８）で表わされる。

上式（ｅｑ８）において、Ｉは単位行列を示し、姿勢行列Ｒ及び単位ベクトルＶの右上の添え字は転置行列を示す記号である。ここで、行列Ｔｔｅｍｐと目標行列Ｔｒｅｆとの間の姿勢の変化量は、行列Ｔｔｅｍｐから定まる姿勢行列Ｒｔｅｍｐと、目標行列Ｔｒｅｆから定まる姿勢行列Ｒｒｅｆの逆行列との積で表される。この積を上式（ｅｑ８）の姿勢行列Ｒに代入することにより算出される角度回転量ΔＡは、姿勢の変化量を表す。ここで、行列Ｔｔｅｍｐから定まる姿勢と、目標行列Ｔｒｅｆから定まる姿勢とが同じならば、姿勢行列Ｒｔｅｍｐと姿勢行列Ｒｒｅｆの逆行列との積をオイラーベクトル表示した場合における角度回転量ΔＡが０となる。

続くステップＳ１６では、位置ずれ量ΔＰの絶対値が第１閾値εｐ未満であってかつ姿勢ずれ量ΔＡの絶対値が第２閾値εａ未満であるとの第１条件、又はカウンタＮｒがその上限値Ｎｔｈ以上であるとの第２条件のいずれかが成立しているか否かを判定する。第１条件は、ステップＳ１１又は後述するステップＳ１９における仮の位置及び姿勢が、目標となる位置及び姿勢に収束したことを判定するための条件である。

ステップＳ１６で否定判定した場合にはステップＳ１７に進み、仮行列Ｔｎｅｗから定まる制御点の仮の位置Ｐｎｅｗから位置ずれ量ΔＰを減算することにより、仮の位置Ｐｎｅｗを更新する。続くステップＳ１８では、仮行列Ｔｎｅｗから定まる仮の姿勢を表す仮姿勢行列Ｒｎｅｗに、姿勢行列Ｒｔｅｍｐと姿勢行列Ｒｒｅｆの逆行列との積を乗算することにより、仮姿勢行列Ｒｎｅｗを更新する。続くステップＳ１９では、更新された仮の位置Ｐｎｅｗと、更新された仮姿勢行列Ｒｎｅｗとにより、仮行列Ｔｎｅｗを更新する。そしてステップＳ１２に戻る。

一方、ステップＳ１６で肯定判定した場合には、ステップＳ２０に進み、直近のステップＳ１２で既に算出されている仮の角度Ｊｔｅｍｐを、第１〜第６回転部１３〜１７の最終的な角度Ｊｒｅｆとして算出する。算出された最終的な角度Ｊｒｅｆは、ＣＰ制御に用いられる。詳しくは、第１〜第６回転部１３〜１７の現在の角度から、第１〜第６回転部１３〜１７の最終的な角度Ｊｔｅｍｐまで動作周期後に制御されるように、各サーボモータが駆動させる。

以上詳述した本実施形態は、以下の利点を有する。

制御点の目標となる位置及び姿勢を表す目標行列Ｔｒｅｆが、仮の位置及び姿勢を表す仮行列Ｔｎｅｗとして設定される。そして、ＤＨパラメータに基づいて仮行列Ｔｎｅｗが逆変換処理されることにより、第１〜第６回転部１３〜１７の仮の角度Ｊｔｅｍｐが算出される。続いて、修正ＤＨパラメータに基づいて仮の角度Ｊｔｅｍｐが順変換処理されることにより、制御点の位置及び姿勢を表す行列Ｔｔｅｍｐが算出され、この行列Ｔｔｅｍｐと目標行列Ｔｒｅｆとに基づいて、位置ずれ量ΔＰ及び姿勢ずれ量ΔＡが算出される。そして、算出された各ずれ量ΔＰ，ΔＡが０に収束するように仮行列Ｔｎｅｗが更新される。そして、各ずれ量ΔＰ，ΔＡが０に収束した場合、既に算出されている仮の角度Ｊｔｅｍｐが第１〜第６回転部１３〜１７の角度として算出される。

上述した本実施形態によれば、目標行列Ｔｒｅｆに修正ＤＨパラメータに基づく逆変換処理を直接施すことなく、目標行列Ｔｒｅｆの位置及び姿勢に対応する各回転部１３〜１７の角度を修正ＤＨパラメータに基づいて算出することができる。

また本実施形態によれば、仮行列Ｔｎｅｗを１回又は数回更新することにより、各回転部１３〜１７の角度を算出することができる。これに対し、ヤコビ行列を用いた繰り返し演算では、演算の繰り返し回数が本実施形態における仮行列Ｔｎｅｗの更新回数の例えば数十倍〜数百倍となる懸念がある。このため本実施形態によれば、ヤコビ行列を用いた繰り返し演算に要する演算量に対してコントローラ３０の演算量を大きく低減することができる。

（その他の実施形態）
なお、上記実施形態を以下のように変更して実施することもできる。

・修正ＤＨパラメータに基づく逆変換処理において、上式（ｅｑ６）の回転行列Ｔγｉを無視してもよい。

・修正ＤＨパラメータにおける平行移動量としては、３つに限らず、２つであってもよい。例えば、上記平行移動量として、Ｘ軸方向における平行移動量ａｉ、及びＺ軸方向における平行移動量ｄｉのみを用いることができる。

・上記実施形態では、角度算出手段により算出された各回転部の角度を、各回転部の角度の制御に用いた。しかしながら、各回転部の角度の制御に限らず、各回転部により発生させる力の制御等、角度算出手段により算出された各回転部の角度を用いる他の制御に用いることもできる。

・上記実施形態において、垂直多関節型のロボット１０に代えて、水平多関節型のロボット等を採用することもできる。

１０…ロボット、１３〜１７…第１〜第６回転部、３０…コントローラ。

Claims (3)

1. 複数の回転部と、複数の前記回転部を順次連結してかつ隣り合う前記回転部を互いに回転可能にする関節と、を含むアームを備えるロボットに適用され、
   前記アームの先端部を制御点とし、前記制御点の目標となる位置及び姿勢を仮の位置及び姿勢として設定する仮設定手段と、
   前記各関節の回転中心軸線がＺ軸とされ、隣り合う前記関節に対応する前記Ｚ軸の共通垂線に沿って前記Ｚ軸から延びる軸線がＸ軸とされ、隣り合う前記関節の前記Ｘ軸方向における距離、隣り合う前記関節に対応する前記Ｚ軸が前記Ｘ軸まわりになす角度、隣り合う前記関節の前記Ｚ軸方向における距離、及び隣り合う前記関節に対応する前記Ｘ軸を前記Ｚ軸まわりになす角度がＤＨパラメータとして定義されており、前記仮設定手段により設定された前記仮の位置及び姿勢を前記ＤＨパラメータに基づいて逆変換処理することにより、前記各回転部の仮の角度を算出する逆変換手段と、
   前記各関節に対して３軸直交座標系が規定され、隣り合う前記関節に対応する前記３軸直交座標系の関係を表すパラメータが、互いに直交する３軸のそれぞれを回転中心軸線とした回転角度量と、互いに直交する３軸のうち少なくとも２軸方向のそれぞれにおける平行移動量とを含む修正ＤＨパラメータとして定義されており、前記逆変換手段により算出された前記各回転部の仮の角度を前記修正ＤＨパラメータに基づいて順変換処理することにより、前記制御点の位置及び姿勢を算出する順変換手段と、
   前記順変換手段により算出された位置及び姿勢と前記目標となる位置及び姿勢とのずれ量を算出するずれ量算出手段と、
   前記ずれ量算出手段により算出されたずれ量が閾値よりも小さくなるまで、前記仮の位置及び姿勢の更新を繰り返す更新手段と、
   前記ずれ量算出手段により算出されたずれ量が前記閾値よりも小さくなった場合に、前記逆変換手段により算出されている前記各回転部の仮の角度を、前記各回転部の角度として算出する角度算出手段と、を備えることを特徴とするロボットの制御装置。
2. 前記ロボットには、前記各回転部のそれぞれを駆動するサーボモータが備えられ、
   前記角度算出手段は、動作周期毎に前記各回転部の角度を算出し、
   前記各回転部の現在の角度から、前記角度算出手段により算出された前記各回転部の角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動手段を備える請求項１に記載のロボットの制御装置。
3. 前記修正ＤＨパラメータとして、前記ロボットを測定して得られた測定値を反映した値が設定される請求項１又は２に記載のロボットの制御装置。

Images