(57)【特許請求の範囲】
【請求項１】
モータで駆動する回転軸を中心に回転するアームを複数有し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、前記モータのコギングトルク補償値を前記フィードバック制御ループに付加させるコギング補償ブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、
前記モータ電流をローパスフィルタで処理して得られる重力補償電流から得られた重力補償電流位相補償角と、前記モータの回転方向から得られた回転方向補正角とを、前記モータの回転角度から得たコギング補償位相角に加算することで、前記多関節型ロボットの姿勢変化に対応した最適なコギング補償位相角を決定する多関節型ロボットの制御方法。
【請求項２】
モータで駆動する回転軸を中心に回転するアームを複数有し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、前記モータのコギングトルク補償値を前記フィードバック制御ループに付加させるコギング補償ブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、
前記回転軸に加わる重力トルクを演算して得られる重力補償電流から得られた重力補償電流位相補償角と、前記モータの回転方向から得られた回転方向補正角とを、前記モータの回転角度から得たコギング補償位相角に加算することで、前記多関節型ロボットの姿勢変化に対応した最適なコギング補償位相角を決定する多関節型ロボットの制御方法。
【請求項３】
コギングトルク補償値にコギング補償ゲインを乗じる手段を付加し、前記コギング補償ゲインが０の時のモータ電流をハイパスフィルタで処理した値からモータとモータ回転位置検出器の取り付け誤差に起因するコギング位相のオフセット量を検出し、コギングトルク位相の補正量に前記オフセット量を加算した後に前記コギング補償ゲインを１にする請求項１または２に記載の多関節型ロボットの制御方法。
【請求項４】
コギングトルク補償値にコギング補償ゲインを乗じる手段を付加し、前記コギング補償ゲインが０の時のモータ電流をハイパスフィルタで処理した値からモータとモータ回転位置検出器の取り付け誤差に起因するコギング位相のオフセット量を検出し、前記オフセット量の絶対値が所定値以内で有れば前記コギング補償ゲインを１にする請求項１または２に記載の多関節型ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、減速機を介してモータにより駆動される多関節型ロボットの制御方法に関するものである。
【背景技術】
【０００２】
近年、溶接やハンドリング等に用いられる垂直多関節型ロボットにおいて、作業精度の向上が求められている。しかし、垂直多関節型ロボットは、合成アーム長が１ｍ以上、構成軸が６軸以上の構成が一般的である。そして、アームのしなりや各軸に搭載されている減速機のバネ成分によりアーム先端が振動しやすい構造になっている。特に、減速機のバネ成分による振動は、大型ロボットでは１０Ｈｚ以下の低い周波数となり、位置制御性能に大きな影響を与える。
【０００３】
一方、バネ成分を有する減速機を駆動するモータには、ロータが永久磁石型のＡＣ同期モータ（通常はブラシレスモータと称される）が、メンテナンス性や制御性の良さから採用されることが多い。しかし、この種類のモータでは、ステータのスロットとロータの磁石が対向するオープンスロット部の磁気エネルギーの変化により、コギングトルクが発生すると考えられ、定格トルクに対して数％のトルク脈動がモータ回転角度に応じて規則的に発生する。
【０００４】
図１は４極のブラシレスモータのコギングトルクの実測値の一例である。モータ１回転当たり２４周期発生している。４極モータでは電気角は１回転当たり４周期になるので、コギングトルクは電気角１周期に対しては６周期発生していることになる。
【０００５】
図２は、このコギングトルクによるロボット先端での振動を評価するための概要図である。図２に示すロボットは、構成軸が６軸の一般的な垂直多関節型ロボットであり、合成アーム長は約１．４ｍである。図２（ａ）において、ロボット先端には溶接トーチ（１０８）が装着されており、ロボットを設置している面と並行に設置された直方体のワーク（１０６）を溶接する場合を考える。ロボットのアームが縮退する方向、つまり図２（ａ）に示す姿勢から図２（ｂ）に示す姿勢へ移動する方向をマイナス動作方向（１０５）とする。そして、ワーク（１０６）の上を一定速度でワーク（１０６）と並行に動作させた時、つまり溶接トーチ（１０８）の先端とワーク（１０６）の距離とを一定に保つ様に動作させた時の溶接トーチ（１０８）の先端の上下振動（１０７）を測定する。なお、この場合は実際に溶接を行うのではなく、溶接トーチ（１０８）の先端とワーク（１０６）との距離を、図示しない距離センサ（例えば渦電流センサ等）を用いて測定する。
【０００６】
ロボットが矢印で示すマイナス動作方向（１０５）の向きで動作するとき、第２軸（１０１）の回転方向（１０２）は時計回りになり、第３軸（１０３）の回転方向（１０４）は反時計回りになる。
【０００７】
さらに、図２（ｂ）の姿勢から、矢印で示すプラス動作方向（１０９）へ反転動作させ、その時の溶接トーチ（１０８）の先端の上下振動（１０７）も同様に評価する。
【０００８】
なお、以降の説明では、説明を簡潔にするために、アーム先端までの回転半径が大きく先端の上下振動（１０７）に影響を及ぼす度合が大きい第２軸（１０１）について説明する。
【０００９】
例えば、ロボットの先端速度が、溶接用途で良く用いられる速度の一例である１．０ｍ／分の場合、第２軸（１０１）の減速機の減速比が１２０前後であると、モータの回転数は２５ｒｐｍ前後となり、その時のコギングトルクの周期は１０Ｈｚ前後となる。このコギングトルクにより第２軸（１０１）の動作速度にコギングトルクと同周波数の速度ムラが生じ、１０Ｈｚ前後の振動として先端の上下振動（１０７）に現れる。そして、この周波数が減速機バネ成分による振動周波数に近づくと、ロボットの振動を抑えることが難しくなる。
【００１０】
一方、ロボットの動作速度が上がり、減速機バネ成分による振動周波数をコギングトルクの周波数が越えた場合、コギングトルクによる力を受けてもバネ成分を含む機械系が応答しなくなるため、溶接トーチ（１０８）を取り付けているアーム先端の振動への影響は殆ど無くなる。そして、減速機バネ成分による振動周波数を越えない低速、例えば図２に示すロボットではモータ最高回転数は４０００〜５０００ｒｐｍであるのでバネ成分の影響を受ける速度は最高速度の１０％未満の低速であり、この低速での補償のみを実施すればよい。
【００１１】
従来、コギングトルクを低減させる方法としては、磁気エネルギーの変化を無くすためにコアレスモータやマグネットにスキュー着磁を施して磁気エネルギーの変化を相殺させる方法といったモータの構造を変更するものがある。しかしながら、コギングトルクを低減させる効果はあるものの、出力トルクや効率の低下を招いていた。
【００１２】
そこで、モータの回転角θＭとその角度でのコギングトルクを相殺する補償トルクを記憶したテーブルから回転角θＭに応じて補償トルクを引き出し、電流指令から減算することでコギングトルクを相殺して振動を抑制する方式も知られている（例えば、特許文献１参照）。なお、本明細書においては、この方式をフィードフォワードコギング補償方式（以後、ＦＦコギング補償方式と略す）と呼ぶことにする。
【００１３】
以下、上述した内容について図面を参照しながら説明する。
【００１４】
図３は、ロボットにおけるモータと減速機をモデル化したものである。モータ取り付けベースとなるアーム１（１）にモータ（２）、減速機（３）、ベアリング（４）が固定され、減速機２次側（７）の回転部に結合された負荷であるアーム２（９）を駆動する。減速機１次側（６）はモータ回転軸（１０）においてモータ内ロータ（５）に結合され、モータ回転速度ωＭ（１１）で回転する。そして、減速機（３）は減速比Ｒｇで、モータ回転速度ωＭ（１１）を負荷回転速度ωＬ（１２）に減速する。
【００１５】
【数１】
0004998159.tif 000002
【００１６】
しかし、減速機（３）では、減速機１次側（６）と減速機２次側（７）の間にバネ成分が存在するので、上記した（数１）の式が成立するのはバネの伸びが一定となった定常状態のみである。このバネ成分のバネ定数をＫｓとして、図３のモデルをブロック線図で表したものが図４である。
【００１７】
図４において、ｉｃｏｍ（１３）はモータ（２）を駆動するモータ電流指令、Ｋｔ（１４）はモータ（２）のトルク定数、１／Ｒｇ（１５、１６）は減速比の逆数、（１７）はモータ伝達関数、（１８）は負荷伝達関数、Ｋｓ（１９）は減速機（３）のバネ定数、θｓ（２０）は減速機１次側（６）と減速機２次側（７）間に発生するねじれ角、（２１）は積分要素、Ｔｄ（２２）は負荷（アーム２）（９）に加わる外力である。
【００１８】
モータコギングトルクＴｃｏｇ（５３）は、モータ電流指令ｉｃｏｍ（１３）にモータトルク定数Ｋｔ（１４）を乗じたモータトルク出力に外乱として加算される。
【００１９】
モータ伝達関数（１７）において、ＪＭ  はモータ内ロータ（５）と減速機１次側（６）を合わせたモータ回転軸（１０）回りの慣性モーメント、ＤＭ  は粘性摩擦係数である。また、負荷伝達関数（１８）においても、ＪＬ  は負荷（アーム２）（９）と減速機２次側（７）を合わせたモータ回転軸（１０）回りの慣性モーメント、ＤＬ  は粘性摩擦係数である。
【００２０】
図３の負荷（アーム２）（９）に対し、従来のコギング補正方法を用いた位置決め制御ループを示したものが図５である。図５において、位置制御ブロック（２７）は、モータに接続されたエンコーダ等によって検出されるモータ回転速度ωＭ（１１）を積分要素（２５）で積分したモータ回転角度θＭ（２４）をモータ位置指令θｃｏｍ（２３）から減算し、位置ゲインＫＰＰ（２６）を乗じてモータ速度指令ωｃｏｍ（２８）を生成する。
【００２１】
【数２】
0004998159.tif 000003
【００２２】
速度制御ブロック（３１）は、モータ速度指令ωｃｏｍ（２８）と、モータ回転速度ωＭ（１１）とにより、下記式でモータ電流指令ｉｃｏｍ０（３２）を生成する。
【００２３】
【数３】
0004998159.tif 000004
ただし、
0004998159.tif 000005
【００２４】
また、コギングトルク補償ブロック（５０）では、コギングトルク１周期分あるいはそれ以上の周期分のコギングトルクをモータトルク定数Ｋｔ（１４）で割った値、即ちコギングトルクに相当するモータ電流データとして予めコギング補償電流テーブル（３４）に記憶しておく。何周期分を記憶するかは、コギングトルクの規則性と制御装置のメモリ容量等を鑑み判断する。
【００２５】
なお、以降の説明では、説明を簡潔にするために、コギング補償電流テーブル（３４）にはコギングトルク１周期分、図１に示すコギングトルクパターン例では、モータ１回転の１／２４周期のデータを２４周期分の平均値として記憶していることを前提とする。
【００２６】
また、モータ回転角度θＭ（２４）のエンコーダ検出精度はモータ１回転当たり２０４８パルスと仮定する。コギングトルク１周期分のパルスは２０４８／２４＝８５．３３３であるので、コギング補償電流テーブル（３４）はモータ検出パルスで８５パルス分のデータを持つことになる。
【００２７】
コギング周期変換（４０）では、モータ回転角度θＭ（２４）をモータ１回転あたりのコギング周期数＝２４で割り、その余りをコギング補償電流テーブル引数θｃｏｇ０（３３）として出力する。コギング補償電流テーブル（３４）は、このコギング補償電流テーブル引数θｃｏｇ０（３３）に従い、コギング電流補償値ｉｃｏｇ１（３５）を出力し、モータ電流指令ｉｃｏｍ０（３２）から減算することで、フィードフォワードでコギングトルクを相殺するモータ電流指令ｉｃｏｍ（１３）を生成する。
【００２８】
以上のように構成することにより、モータコギングトルクによる振動発生を抑えることができる。
【特許文献１】特開昭６２−１６３５９１号公報
【発明の開示】
【発明が解決しようとする課題】
【００２９】
コギングトルクは定格トルクの数パーセント程度と小さいため、測定時にはモータのベアリングの摩擦の影響を無視できず、正確に測定するには困難が伴う。また、例えば特開平５−１４９８０３号公報に記載されているように、コギングトルクの測定は、自らのモータには通電せず、外部の動力により一定の低速度で回転させる静的な条件で測定される。
【００３０】
また、モータのコギング位相とエンコーダの基準位置（原点位置）は、個体に関係なく常に同じ位相であることが必要である。しかし、エンコーダのモータへの取り付け誤差により、モータ機械角で±２°（コギングトルク周期では±２×２４＝±４８°）程度の誤差があることが仕様として記述されている。
【００３１】
これらの理由により、測定誤差や個体差を含むコギングデータを用い、さらに自らのモータに電流を流してトルクを発生させた場合に、従来のＦＦコギング補償方式では振動をフィードバックして最適位相を調整する手段が無く、十分な振動抑制効果が得られる確証はない。
【００３２】
そこで、ロボットの姿勢が変化し、重力によるトルクを支えるために必要なモータトルクが増減し、モータ指令電流ｉｃｏｍ０（３２）が増減した場合の振動抑制効果について評価を行った。
【００３３】
図６、図７はそれぞれ、図２に示すロボットの第２軸（１０１）が時計方向に６０°、１２０°回転した位置での振動評価の姿勢例を示したものである。この時、第２軸（１０１）に加わる重力は、時計回りをプラス動作方向とすると、図２ではマイナス動作方向、図６で０に近づき、図７でプラス動作方向に反転する。
【００３４】
図５に示す方式は、図６に示す姿勢付近では振動抑制効果があるものの、図２や図７に示す姿勢で重力を支えるための重力トルク補償電流が第２軸（１０１）のモータに流れている場合は、後述するように振動抑制効果が減少した。
【００３５】
そこで、図８に示すように、コギング補償電流テーブル引数θｃｏｇ０（３３）に位相シフトθｏｆｆ３（６２）をコギング位相シフト手段（６０）により加算する方法を追加した。図８では、コギング位相シフト手段（６０）では、モータ機械角で±５°（コギングトルク±１／３周期、エンコーダパルスで±２８パルス）程度の範囲で位相シフトθｏｆｆ３（６２）を変化させる。図２に示すロボットの姿勢で、この位相シフトθｏｆｆ３（６２）と先端の上下振動（１０７）との関係を示したものが図９である。
【００３６】
図９において、縦軸はコギング電流補償値ｉｃｏｇ１（３５）を加算しない状態、つまりＦＦコギング補償をしない状態での先端の上下振動（１０７）の平均振幅に対するＦＦコギング補償を実施した場合の改善率である。改善率が最も大きくなる位相シフトθｏｆｆ３（６２）の最適値は、０°ではなく、マイナス動作方向（１０５）で約−２．２°、プラス動作方向（１０９）で約−２．６  °であり、静的な条件で計測しテーブル化された位相とは一致せず、動作方向で最適位相シフトに差が見られる。
【００３７】
さらに図９からは、最適位相シフト量から±２．５°程度位相シフト量がずれると、振動抑制効果が殆ど無くなり、それ以上ずれると振動を増幅する恐れがあることがわかる。
【００３８】
そこで、上記した評価を、図２や図７に示すロボットの姿勢のように、第２軸（１０１）の角度を変え、第２軸（１０１）にかかる重力トルクを変えることで、第２軸（１０１）のモータに流れる重力補償電流を変化させて実施する。その時の重力補償電流とコギング補償の最適位相シフト量の関係を示したものが図１０である。
【００３９】
図１０において、マイナス動作方向（１０５）と、プラス動作方向（１０９）のデータのそれぞれの近似曲線は（７０）と（７１）であり、最適位相シフト量は重力補償電流に依存して±２°程度変動することが判明した。
【００４０】
エンコーダのモータへの取り付け誤差（モータ機械角で±２°）が存在すると、コギング補償最適位相は、上記重力補償電流による変動と合わせ、最大±４°の誤差が発生し、図９で考察したように±２．５°の範囲を越えるため、振動を抑制するどころか悪化させる恐れがある。
【００４１】
つまり、静的な条件で計測し、テーブル化されたコギング補償電流テーブル（３４）を用いる図５で示した従来のＦＦコギング補償方式では、重力補償電流による最適位相の変化やエンコーダ取り付け誤差による最適位相の誤差に対応した位相シフトが行われず、十分な振動抑制効果を発揮できないことが判明した。
【００４２】
本発明は、上記課題を解決するものであり、重力補償電流によるコギング補償の最適位相の変化あるいはエンコーダ取り付け誤差による最適位相誤差が存在しても、コギングトルクに起因する振動を抑制することができるロボットの制御方法を実現することを目的とする。
【課題を解決するための手段】
【００４３】
上記課題を解決するために、本発明の多関節型ロボットの制御方法は、モータで駆動する回転軸を中心に回転するアームを複数有し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、前記モータのコギングトルク補償値を前記フィードバック制御ループに付加させるコギング補償ブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、前記モータ電流をローパスフィルタで処理して得られる重力補償電流から得られた重力補償電流位相補償角と、前記モータの回転方向から得られた回転方向補正角とを、前記モータの回転角度から得たコギング補償位相角に加算することで、前記多関節型ロボットの姿勢変化に対応した最適なコギング補償位相角を決定するものである。
【００４４】
また、本発明の多関節型ロボットの制御方法は、モータで駆動する回転軸を中心に回転するアームを複数有し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、前記モータのコギングトルク補償値を前記フィードバック制御ループに付加させるコギング補償ブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、前記回転軸に加わる重力トルクを演算して得られる重力補償電流から得られた重力補償電流位相補償角と、前記モータの回転方向から得られた回転方向補正角とを、前記モータの回転角度から得たコギング補償位相角に加算することで、前記多関節型ロボットの姿勢変化に対応した最適なコギング補償位相角を決定するものである。
【００４５】
また、本発明の多関節型ロボットの制御方法は、上記に加えて、コギングトルク補償値にコギング補償ゲインを乗じる手段を付加し、前記コギング補償ゲインが０の時のモータ電流をハイパスフィルタで処理した値からモータとモータ回転位置検出器の取り付け誤差に起因するコギング位相のオフセット量を検出し、コギングトルク位相の補正量に前記オフセット量を加算した後に前記コギング補償ゲインを１にするものである。
【００４６】
また、本発明の多関節型ロボットの制御方法は、上記に加えて、コギングトルク補償値にコギング補償ゲインを乗じる手段を付加し、前記コギング補償ゲインが０の時のモータ電流をハイパスフィルタで処理した値からモータとモータ回転位置検出器の取り付け誤差に起因するコギング位相のオフセット量を検出し、前記オフセット量の絶対値が所定値以内で有れば前記コギング補償ゲインを１にするものである。
【発明の効果】
【００４７】
以上のように、本発明の多関節型ロボットの制御方法によれば、重力補償電流によるコギング補償の最適位相の変化あるいはエンコーダ取り付け誤差による最適位相誤差が存在しても、コギングトルクに起因する振動を抑制することができる。
【発明を実施するための最良の形態】
【００４８】
（実施の形態１）
以下、本発明の実施の形態について、主に図１１と１２を用いて説明する。なお、図１から図１０と同様の箇所については同一の符号を付して詳細な説明を省略する。
【００４９】
図１１は本実施の形態における制御ブロックを示す図である。
【００５０】
重力補償電流とコギング補償最適位相シフト量の関係を示した図１０において、マイナス動作方向（１０５）と、プラス動作方向（１０９）のそれぞれの近似曲線である（７０）と（７１）は、全体的なオフセットはあるものの、重力補償電流に対する最適位相シフト量の変化は同等である。そこで、この最適位相シフト量を重力補償電流と動作方向から計算し、ＦＦコギング補償する際に、コギング補償テーブル引数θｃｏｇ０（３３）を補正する。最適位相補正角をΔθｇｄ（５４）とすると、下記の１次関数で表すことができる。
【００５１】
【数４】
0004998159.tif 000006
ただし、Δθｇ：重力補償電流補正角、Δθｄ：回転方向補正角、
ｉｇｆ：重力トルク補償電流推定値
0004998159.tif 000007
【００５２】
ただし、ａｇ１，ａｇ２，ｇｔｈ１，ｇｔｈ２，−ｇｈｔ１，−ｇｔｈ２，ｂｇ２，−ｂｇ２，ｂｇ３，−ｂｇ３，Δθｄｐ，Δθｄｍは、図１０の２本の近似曲線（７０）、（７１）と一致するように定める。
【００５３】
（数４）の式（６）をグラフ化したものが図１２である。（数４）の式（６）におけるｉｇｆは、図１１に示す重力トルク補償電流推定値ｉｇｆ（３７）であり、モータ電流指令ｉｃｏｍ（１３）から重力補償電流演算ブロック（３６）で演算される。
【００５４】
図１３（ａ）は、ロボットの第２軸（１０１）が時計回り（１０２）に等角速度で回転した場合の、アーム回転角度（＝θＭ／減速比、減速比＝１２１）とモータ電流指令ｉｃｏｍ（１３）との関係を示したものであり、横軸がアーム回転角度、縦軸がモータ電流指令である。
【００５５】
図１３（ｂ−１）、（ｂ−２）は、それぞれ図１３（ａ）のアーム回転角が０°近辺と１２０°近辺を角度方向に拡大してモータ回転角度θＭ（２４）とモータ電流指令ｉｃｏｍ（１３）の関係を示したものであり、横軸がモータ回転角度、縦軸がモータ電流指令である。図１１の重力補償電流演算ブロック（３６）において、モータ電流指令ｉｃｏｍ（１３）をローパスフィルタで処理し、図１３（ｃ−１）、（ｃ−２）に示す重力トルク補償電流推定値ｉｇｆ（３７）を抽出する。なお、ローパスフィルタで処理する理由は、重力に関係のないコギング成分を取り除くためである。なお、図１３（ｃ−１）、（ｃ−２）において、横軸はモータ回転角度、縦軸は重力補償電流である。また、図１３（ｄ）において、横軸はアーム回転角度、縦軸は重力補償電流である。
【００５６】
図１１の重力補償電流位相補償（３８）は、重力補償電流推定値ｉｇｆ（３７）から重力補償電流位相補償角Δθｇ（３９）を（数４）の式（６）に従い出力する。
【００５７】
図１１の動作方向位相補償ブロック（４２）は、（数４）の式（７）に従い、回転方向補正角Δθｄ（４３）を出力する。そして、（数４）の式（５）に従い、重力補償電流位相補償角Δθｇ（３９）を加算して最適位相補正角Δθｇｄ（５４）を計算する。
【００５８】
この最適位相補正角Δθｇｄ（５４）をコギング補償電流テーブル引数θｃｏｇ０（３３）に加算することで、コギング補償電流テーブル修正引数θｃｏｇ１（４４）を得る。
【００５９】
【数５】
0004998159.tif 000008
【００６０】
このコギング補償電流テーブル修正引数θｃｏｇ１（４４）をコギング補償電流テーブル（３４）に入力することで、重力補償電流や動作方向に対応した位相補償がされたコギング電流補償値ｉｃｏｇ１（３５）が出力され、重力補償電流や動作方向に関わらず、最適なＦＦコギング補償が実施され振動を抑制することができる。なお、モータ指令電流ｉｃｏｍ０（３２）とコギング電流補償値ｉｃｏｇ１（３５）とによりモータ電流指令ｉｃｏｍ（１３）が求められる。
【００６１】
（実施の形態２）
本実施の形態について、図１４を用いて説明する。図１４は本実施の形態の制御ブロック図を示すものである。なお、実施の形態１と同様の箇所については同一の符号を付して詳細な説明を省略する。
【００６２】
図１１を用いて説明した実施の形態１では、ローパスフィルタによりモータ電流ｉｃｏｍ（１３）からコギング周波数成分を取り除くことで重力トルク補償電流推定値ｉｇｆ（３７）を計算していた。この方法では、処理が簡便な反面、モータ回転速度ωＭ（１１）が変化すればコギングと重力補償電流の周波数もそれに比例して変動するので、ローパスフィルタのカットオフ周波数選定に関して調整する必要がある。
【００６３】
また、コギング成分を完全に取り除くのは困難であり、フィルタによる位相遅れも発生するので、コギング補償の効果が少なくなる可能性がある。
【００６４】
図１４においては、自軸のモータ回転角度θＭ（２４）と他軸のモータ回転角度θＭ（４１）から、重力補償電流演算ブロック（３６）によりロボットの動力学演算を用いて自軸に印加される重力トルクを演算し、重力トルク補償電流推定値ｉｇｆ（３７）を求めるようにしている。
【００６５】
この方法では、動力学演算に費やす処理時間が増えるが、フィルタを用いた場合の問題点を解消でき、より安定した性能を発揮することが出来る。
【００６６】
（実施の形態３）
本実施の形態について、図１５から図１９を用いて説明する。なお、実施の形態１や実施の形態２と同様の箇所については同一の符号を付して詳細な説明を省略する。
【００６７】
図１５は本実施の形態の制御ブロック図を示すものであり、図１１を用いて実施の形態１で説明した方式をベースに、図１８を用いて後述するエンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）を自動的に補正するブロックを追加したものである。
【００６８】
図１５において、コギング補償ゲインＫｃｏｇ（５１）の初期値を０に設定する。従って、ＦＦコギング補償電流ｉｃｏｇ２（５２）は０となり、ＦＦコギング補償が実施されない状態となる。
【００６９】
この状態では、ＦＦコギング補償は無いが、通常のフィードバック制御はかかっている。そのため、モータコギングトルクＴｃｏｇ（５３）に起因する振動が実際のモータ回転角度θＭ（２４）に現れ、モータ位置指令θｃｏｍ（２３）との差を解消するために、結果的にモータ電流指令ｉｃｏｍ（１３）の成分にコギングトルクと同じ周波数のコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）が含まれることになる。
【００７０】
図１６（ａ）は、第２軸（１０１）が時計回り（１０２）に等角速度で回転した場合の、アーム回転角度（＝θＭ／減速比、減速比＝１２１）とモータ電流指令ｉｃｏｍ（１３）との関係を示したものである。
【００７１】
図１６（ｂ−１）は、アーム回転角度が０°近辺を角度方向に拡大したもので、モータ回転角度θＭ（２４）とモータ電流指令ｉｃｏｍ（１３）の関係を示したものである。図１５に示すコギング成分抽出ブロック（４５）において、重力補償電流成分に比べ高周波であるコギング成分を抽出するために、電流指令ｉｃｏｍ（１３）にハイパスフィルタ処理を行い、極性を反転させ、図１６（ｃ−１）に示すコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）を抽出する。
【００７２】
このコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）は、実際のモータ回転角度θＭ（２４）とモータ位置指令θｃｏｍ（２３）との差が生じてから、その差を解消するためにモータ電流指令ｉｃｏｍ（１３）が変化することにより生じるので、その位相は、図１１を用いて説明したコギングトルク電流補償値ｉｃｏｇ１（３５）を施した場合の位相補正角Δθｇｄ（５４）より、フィードバック位相遅れ（以後、ＦＢ位相遅れと略す）Δθｄｌｙ分遅れることになる。
【００７３】
図１７に、図１４に示すブロック図におけるコギングトルク電流補償値ｉｃｏｇ１（３５）と、図１５に示すブロック図におけるコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）の関係を示す。
【００７４】
ここで、モータの個体差、即ち、エンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）が存在する場合を考える。
【００７５】
図１８は、図１１を用いて説明した方式にエンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）が存在する場合の対応を加えたものであり、下式で計算されるコギング補正値θｃｏｇ２（４９）をコギング補償電流テーブル（３４）の引数としている。
【００７６】
【数６】
0004998159.tif 000009
【００７７】
図１９は、図１８におけるコギングトルク電流補償値ｉｃｏｇ１（３５）と図１５におけるコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）の関係を示したものである。図１９に示すθｄｌｙはフィードバック制御による位相遅れを表したものであり、その遅れは制御ループの応答性に起因するので、モータ個体差には殆ど影響されず予め測定可能である。
【００７８】
この場合、エンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）は個体で別々であるので、図１８でＦＦコギングトルク補償を施す場合、この個体差が無視できない大きさであると、個別にエンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）を測定しパラメータとして持つ必要が出てくる。
【００７９】
このことは、製造段階の工数を増加させるだけでなく、ロボットとその制御パラメータを持つ制御装置のペアリングが必要となり、互換性の面でも不利となる。
【００８０】
そこで、図１５に示すコギング位相検出ブロック（４７）において、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）の位相を計測することにより、自動的にエンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）のエンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）を求める。
【００８１】
図１９において、最適位相補正角Δθｇｄ（５４）とＦＢ位相遅れΔθｄｌｙの個体差は、エンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）に比べると無視できるほど小さい。従って、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）に対し常に（Δθｄｌｙ−Δθｇｄ）の位相補正を行いながら、コギング周期（＝モータ回転角１５°毎）で平均し、その波形のゼロクロス点を計測すれば、エンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）を求めることが出来る。
【００８２】
この内容を具体的に説明したものが、図１６と図２０であり、図１６はΔθｏｆｆ＝０°の場合を示し、図２０はΔθｏｆｆ＝１．８°の場合を示している。
【００８３】
ここで、図１６（ａ）と図２０（ａ）は、横軸がアーム回転角度であり、縦軸がモータ電流指令である。また、図１６（ｂ−１）と図１６（ｂ−２）と図２０（ｂ−１）と図２０（ｂ−２）は、横軸がモータ回転角度であり、縦軸がモータ電流指令である。また、図１６（ｃ−１）と図１６（ｃ−２）と図２０（ｃ−１）と図２０（ｃ−２）は、横軸がモータ回転角度であり、縦軸がコギングトルクＦＢ補償電流ｉｃｏｇｆである。また、図１６（ｄ−１）と図１６（ｄ−２）と図２０（ｄ−１）と図２０（ｄ−２）は、横軸がモータ回転角度であり、縦軸がコギングトルクＦＢ補償電流ｉｃｏｇｆ２である。また、図１６（ｅ）と図２０（ｅ）は、横軸がモータ回転角度であり、縦軸がコギングトルクＦＢ補償電流平均値である。
【００８４】
図１６（ｂ−２）は、アーム回転角が１２０°近辺を角度方向に拡大したもので、モータ回転角度θＭ（２４）とモータ電流指令ｉｃｏｍ（１３）の関係を示したものである。図１５におけるコギング成分抽出ブロック（４５）で、モータ電流指令ｉｃｏｍ（１３）にハイパスフィルタを施し、反転させ、図１６（ｃ−２）に示すコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）を抽出する。
【００８５】
ただし、このままではノイズが多く、ゼロクロス点を求めることが難しいので、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）をコギング周期（＝モータ回転角１５°毎）で平均することを考える。
【００８６】
図１６（ｃ−１）と（ｃ−２）では、重力電流補償が異なるので、最適位相補正角θｇｄ（５４）が異なる。そのまま平均化すれば、位相がずれた正弦波の足し合わせとなり、平均化の意味が無くなる。そこで、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）の（Δθｄｌｙ−Δθｇｄ）の位相補正を行ったものがｉｃｏｇｆ２であり、図１６（ｄ−１）、（ｄ−２）に示したものを平均化する。
【００８７】
アーム回転角がどの角度においても、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）に対し（Δθｄｌｙ−Δθｇｄ）の位相補正を行えば、図１６（ｄ−１）、（ｄ−２）と同じ位相になるので、アーム角度が０から１２０°まで移動する間も連続的に平均化できる。
【００８８】
ｉｃｏｇｆ２をコギング周期（＝モータ回転角１５°毎）で平均したものが図１６（ｅ）である。図１６（ｅ）におけるゼロクロス点をθ０とすると、コギング周期の中点は１５／２＝７．５°であるので、エンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）は以下の式で求められる。
【００８９】
【数７】
0004998159.tif 000010
【００９０】
図１６（ｅ）においては、ゼロクロス点θ０＝７．５°であるので、Δθｏｆｆｄ＝０．０と求めることが出来る。
【００９１】
また、図２０は、Δθｏｆｆ＝１．８°の場合であり、（Δθｄｌｙ−Δθｇｄ）の位相補正を行った図２０（ｄ−１）から図２０（ｄ−２）の動作間のｉｃｏｇｆ２を平均すると、図２０（ｅ）に示す様にゼロクロス点θ０＝９．３°となり、式（１０）よりΔθｏｆｆｄ＝１．８°と計算される。
【００９２】
以上の方法により、予めエンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）を調べなくても、モータ電流指令ｉｃｏｍ（１３）より推定出来ることを示した。
【００９３】
ここでは、説明を簡単にするために、第２軸（１０１）が時計回り（１０２）に等角速度で回転した場合を示したが、コギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）に対し常に（Δθｄｌｙ−Δθｇｄ）の位相補正を行っているので、一定の姿勢や速度でなくても推定は可能である。
【００９４】
ただし、モータ回転速度が上がり、コギングによる振動がメカの共振周波数より高くなり、振動としての現象が現れない場合は、モータ電流指令ｉｃｏｍ（１３）の成分にコギングトルクと同じの周波数のコギングトルクＦＢ補償電流成分ｉｃｏｇｆ（４６）は含まれないので推定は不可能となる。
【００９５】
しかし、元々コギングによる振動がメカの共振周波数より低い場合の振動抑制を行うので、この推定自体もその範囲内で実施する。
【００９６】
このエンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）を求めた後で、図１５において、コギング位相検出ブロック（４７）は、コギング補償ゲインＫｃｏｇ（５１）にオン信号Ｋｏｎ（５６）を送り、Ｋｃｏｇ（５１）は１に設定すると共に、以下の式で計算されるコギング補正値θｃｏｇ２（４９）がコギング補償電流テーブル（３４）に入力され、個体差を示すΔθｏｆｆ（５５）まで考慮したＦＦコギング補償電流ｉｃｏｇ２（５２）が電流指令ｉｃｏｍ０（３２）に加算される。
【００９７】
【数８】
0004998159.tif 000011
【００９８】
ただし、Ｋｃｏｇ（５１）を０から１に急激に切り換えると、モータ電流指令ｉｃｏｍ（１３）が不連続となり、それが原因となる振動を誘発しかねない。そこでオン信号Ｋｏｎ（５６）を受けた後、Ｋｃｏｇ（５１）は０から徐々に増加させて１に到達させることが望ましい。
【００９９】
（実施の形態４）
本実施の形態について、図２１を用いて説明する。図２１は本実施の形態の制御ブロック図を示すものである。なお、実施の形態１からや実施の形態３と同様の箇所については同一の符号を付して詳細な説明を省略する。
【０１００】
図１５を用いた説明した実施の形態３では、重力トルク補償電流推定値ｉｇｆ（３７）を、ローパスフィルタによりモータ電流ｉｃｏｍ（１３）からコギング周波数成分を取り除くことで計算していたが、図２１においては、自軸のモータ回転角度θＭ（２４）と他軸のモータ回転角度θＭ（４１）から、ロボットの動力学演算を用いて自軸に印加される重力トルクを演算し、補償電流推定値ｉｇｆ（３７）を求めるようにしている。
【０１０１】
この方法では、動力学演算に費やす処理時間が増えるが、フィルタを用いた場合の問題点を解消でき、より安定した性能を発揮することが出来る。
【０１０２】
（実施の形態５）
本実施の形態について、図２２を用いて説明する。図２２は本実施形態の制御ブロック図を示すものである。なお、実施の形態１からや実施の形態４と同様の箇所については同一の符号を付して詳細な説明を省略する。
【０１０３】
コギング位相検出ブロック（４７）は、（数７）の式（１０）でエンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）を求めた後、以下の条件が満たされれば、オン信号Ｋｏｎ（５６）を発行し、Ｋｃｏｇ（５１）を０から１にする。
【０１０４】
【数９】
0004998159.tif 000012
ただし、
θｔｈ：コギング補償有効判定  位相閾値
【０１０５】
実施の形態３では、エンコーダのモータへの取り付け誤差の推定値Δθｏｆｆｄ（４８）で位相を補正してコギング補償を実施する方法であったのに対して、本実施の形態では、Δθｏｆｆｄ（４８）が所定値θｔｈ以内であれば、位相を補正することなくコギング補償を実施する。この方法を取れば、エンコーダのモータへの取り付け誤差Δθｏｆｆ（５５）の個体差への対応力は弱くなる反面、エンコーダのモータへの取り付け誤差の推定値θｏｆｆｄ（４８）の推定誤差の大きさがコギング補償位相へ直接反映されないため、より安定した動作が得られる。
【０１０６】
（実施の形態６）
本実施の形態について、図２３を用いて説明する。図２３は本実施の形態の制御ブロック図を示すものである。なお、実施の形態１からや実施の形態５と同様の箇所については同一の符号を付して詳細な説明を省略する。
【０１０７】
図２２を用いて説明した実施の形態５では、重力トルク補償電流推定値ｉｇｆ（３７）を、ローパスフィルタによりモータ電流ｉｃｏｍ（１３）からコギング周波数成分を取り除くことで計算していたが、図２３においては、自軸のモータ回転角度θＭ（２４）と他軸のモータ回転角度θＭ（４１）から、重力補償電流演算ブロック（３６）によりロボットの動力学演算を用いて自軸に印加される重力トルクを演算し、補償電流推定値ｉｇｆ（３７）を求めるようにしている。
【０１０８】
この方法では、動力学演算に費やす処理時間が増えるが、フィルタを用いた場合の問題点を解消でき、より安定した性能を発揮することが出来る。
【産業上の利用可能性】
【０１０９】
本発明の多関節型ロボットの制御方法は、ＦＦコギング補償方式において、重力補償電流によるコギング補償の最適位相の変化あるいはエンコーダ取付誤差による最適位相誤差が存在しても、コギングトルクに起因する振動を抑制することができるので、ロボットの制御方法として産業上有用である。
【図面の簡単な説明】
【０１１０】
【図１】モータのコギングトルクの一例を示す図
【図２】コギングトルクに起因するロボット先端振動の評価方法を説明するための参考図
【図３】ロボットの減速機のバネ成分を示す概略構成図
【図４】ロボットの減速機のバネ成分をモデル化したブロック図
【図５】従来のコギング補償を含む位置制御ループを示すブロック図
【図６】コギングトルクに起因するロボット先端振動の評価方法を説明するための参考図
【図７】コギングトルクに起因するロボット先端振動の評価方法を説明するための参考図
【図８】コギング補償位相角と振動抑制効果の関係を評価する位置制御ループを示すブロック図
【図９】コギング補償位相シフト量と振動抑制改善率の関係を示す図
【図１０】第２軸モータにおける重力トルク補償電流とコギング補償最適位相の関係を示す図
【図１１】本発明の実施の形態１におけるコギング補償を含む位置制御ループを示すブロック図
【図１２】重力補償電流補正角を求める式を図式化した図
【図１３】（ａ）アーム回転角度とモータ電流指令の関係を示す図（ｂ−１）モータ回転角度とモータ電流指令の関係を示す図（ｂ−２）モータ回転角度とモータ電流指令の関係を示す図（ｃ−１）モータ回転角度とモータ電流指令の関係を示す図（ｃ−２）モータ回転角度とモータ電流指令の関係を示す図（ｄ）アーム回転角度とモータ電流指令の関係を示す図
【図１４】本発明の実施の形態２におけるコギング補償を含む位置制御ループを示すブロック図
【図１５】本発明の実施の形態３におけるコギング補償を含む位置制御ループを示すブロック図
【図１６】（ａ）アーム回転角度とモータ電流指令の関係を示す図（ｂ−１）モータ回転角度とモータ電流指令の関係を示す図（ｂ−２）モータ回転角度とモータ電流指令の関係を示す図（ｃ−１）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｃ−２）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｄ−１）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｄ−２）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｅ）モータ回転角度とコギングトルクＦＢ補償電流平均値の関係を示す図
【図１７】モータ回転角度に対する図１４のコギングトルク電流補償値と図１５のコギングトルクＦＢ補償電流成分の関係を示す図
【図１８】図１１のブロック図にエンコーダのモータへの取り付け誤差が存在する場合の対応を加えたブロック図
【図１９】モータ回転角度に対する図１８のコギングトルク電流補償値とコギングトルクＦＢ補償電流成分の関係を示す図
【図２０】（ａ）アーム回転角度とモータ電流指令の関係を示す図（ｂ−１）モータ回転角度とモータ電流指令の関係を示す図（ｂ−２）モータ回転角度とモータ電流指令の関係を示す図（ｃ−１）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｃ−２）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｄ−１）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｄ−２）モータ回転角度とコギングトルクＦＢ補償電流の関係を示す図（ｅ）モータ回転角度とコギングトルクＦＢ補償電流平均値の関係を示す図
【図２１】本発明の実施の形態４におけるコギング補償を含む位置制御ループを示すブロック図
【図２２】本発明の実施の形態５におけるコギング補償を含む位置制御ループを示すブロック図
【図２３】本発明の実施の形態６におけるコギング補償を含む位置制御ループを示すブロック図
【符号の説明】
【０１１１】
１      アーム１
２      モータ
３      減速機
４      ベアリング
５      モータ内ロータ
６      減速機１次側
７      減速機２次側
８      バネ
９      アーム２（負荷）
１０    モータ回転軸
１１    モータ回転速度ωＭ
１２    負荷回転速度ωＬ
１３    モータ電流指令ｉｃｏｍ
１４    モータトルク定数Ｋｔ
１５    減速比の逆数
１６    減速比の逆数
１７    モータ伝達関数
１８    負荷伝達関数
１９    バネ定数Ｋｓ
２０    ねじれ角θｓ
２１    積分要素
２２    外力Ｔｄ
２３    モータ位置指令θｃｏｍ
２４    モータ回転角度θＭ
２５    積分要素
２６    位置ゲインＫＰＰ
２７    位置制御ブロック
２８    モータ速度指令ωｃｏｍ
２９    速度比例ゲイン  ＫＰ
３０    速度積分ゲイン  ＫＩ
３１    速度制御ブロック
３２    モータ電流指令ｉｃｏｍ０（速度ループブロック出力）
３３    コギング補償電流テーブル引数  θｃｏｇ０
３４    コギング補償電流テーブル
３５    コギング電流補償値  ｉｃｏｇ１
３６    重力補償電流演算ブロック
３７    重力トルク補償電流推定値ｉｇｆ
３８    重力補償電流位相補償
３９    重力補償電流位相補償角Δθｇ
４０    コギング周期変換
４１    他軸のモータ回転角度θＭ
４２    動作方向位相補償ブロック
４３    回転方向補正角  Δθｄ
４４    コギング補償電流テーブル修正引数  θｃｏｇ１
４５    コギング成分抽出ブロック
４６    コギングトルクＦＢ補償電流成分  ｉｃｏｇｆ
４７    コギング位相検出ブロック
４８    エンコーダのモータへの取り付け誤差の推定値  Δθｏｆｆｄ
４９    コギング補正値  θｃｏｇ２
５０    コギングトルク補償ブロック
５１    コギング補償ゲイン  Ｋｃｏｇ
５２    ＦＦコギング補償電流  ｉｃｏｇ２
５３    モータコギングトルク  Ｔｃｏｇ
５４    最適位相補正角  Δθｇｄ
５５    エンコーダのモータへの取り付け誤差Δθｏｆｆ
５６    オン信号Ｋｏｎ
６０    コギング位相シフト手段
６２    位相シフトθｏｆｆ３
７０    近似曲線（マイナス動作方向）
７１    近似曲線（プラス動作方向）
７５    ロボットのモータ、減速機及び負荷を示すブロック
１０１  第２軸
１０２  回転方向
１０３  第３軸
１０４  回転方向
１０５  マイナス動作方向
１０６  ワーク
１０７  先端の上下振動
１０８  溶接トーチ
１０９  プラス動作方向
【図１】
0004998159.tif 000013
【図２】
0004998159.tif 000014
【図３】
0004998159.tif 000015
【図４】
0004998159.tif 000016
【図５】
0004998159.tif 000017
【図６】
0004998159.tif 000018
【図７】
0004998159.tif 000019
【図８】
0004998159.tif 000020
【図９】
0004998159.tif 000021
【図１０】
0004998159.tif 000022
【図１１】
0004998159.tif 000023
【図１２】
0004998159.tif 000024
【図１３】
0004998159.tif 000025
【図１４】
0004998159.tif 000026
【図１５】
0004998159.tif 000027
【図１６】
0004998159.tif 000028
【図１７】
0004998159.tif 000029
【図１８】
0004998159.tif 000030
【図１９】
0004998159.tif 000031
【図２０】
0004998159.tif 000032
【図２１】
0004998159.tif 000033
【図２２】
0004998159.tif 000034
【図２３】
0004998159.tif 000035