JP2012130214A - モータ制御装置及びモータ制御方法 - Google Patents

Abstract

【課題】 剛性の低いベルトを力伝達手段として含む運動機構におけるベルトの弾性変形の補正を行って、軌道と軌跡とのずれ量を小さくできるモータ制御装置及び制御方法を提供する。
【解決手段】 位置指令をプレフィルタ３に通し、その後位置指令と位置フィードバックとの偏差をとり、位置制御部２に通して速度指令を算出する。速度指令と速度との偏差をとり、速度制御部７に通し、トルク指令を算出する。このトルク指令を元に、トルク制御部８内の電力変換器を通してモータを駆動する。また、トルク指令とモータ速度から負荷トルクオブザーバ９により負荷トルクを推定し、この負荷トルクに対し、運動機構の作用点の軌道に応じた弾性変形補償のための補償ゲインを乗算し補償量を求める。この補償量を振動抑制フィルタ１３に通して位置偏差に加算する。
【選択図】 図１

Description

本発明は、ベルトを力伝達手段として含む運動機構を負荷とするモータの制御装置及び制御方法に関するものである。

ベルトを力伝達手段として含む運動機構は、ロボットなどの機械に採用されている。このような運動機構を負荷とするモータでは、運動機構の作用点の位置制御の精度を高めることが要求されている。例えば、図４は、モータを２台用いた多関節ロボットの構成例である。モータＭ１により、ベルトＢ１を介して腕１０１を駆動し、モータＭ２によりベルトＢ２を介してロボットの腕１０２の先端に設けた作用部１０３の先端部（作用点）を予め定めた軌道に沿うように移動させる。このようなロボットで、モータＭ１及びＭ２を駆動して腕１０１及び１０２を動かした場合、双方のモータＭ１及びＭ２に外力が加えられ、ロボットの作用部１０３の先端の軌跡が本来の軌道からずれてしまう場合がある。これは、ベルトがかかっている一方の軸が他方の軸からの力の影響を受け、ベルトＢ２の弾性変形に応じた位置の誤差を生ずるためである。搬送ロボットなどでロボットの作用部１０３の軌跡が本来の軌道から大きくずれると、物を搬入するときに周囲の他の物に衝突する恐れがある。したがって軌跡と軌道とのずれ量をできるだけ小さくする必要がある。そこで従来は、外力の影響を受けない程度の速度でモータを駆動するようにしていた。

このような機械系の弾性変形の影響を補正した例として、減速機のねじれを補正した特許文献１（特許第３６５４４７５号公報）に示すものがある。この例では、モータ電流を検出して、あらかじめ計測してあるモータ電流とねじれの関係に基づきねじれ補正量を算出し、位置指令に補正を加えている。

また、位置決め制御装置の弾性変形を補正した例として、特許文献２（特許第３９８２３０８号公報）に示すものがある。この例では、外乱推定部内で推定される推定外乱力を基に、制御対象が目標位置に到達した時点で位置決め機構内に残存する弾性変形量を推定し、目標位置に弾性変形量を加えた位置を新たな目標位置として位置決め制御を行い、新たな目標位置に制御対象が達した後、弾性変形量分を戻す位置制御を行っている。

特許第３６５４４７５号公報 特許第３９８２３０８号公報

しかし、特許文献１の技術では、モータが発生するトルクに応じたねじれ分を補正しているのみである。特許文献１は、外乱によるベルトの弾性変形を補償する技術を示唆するものではない。また、特許文献２の技術は、制御対象の位置決め精度の改善を目標としているため、位置決め時の補正を中心とした補償になっている。そのため、特許文献２には、位置決めに至る過程での制御対象の位置の軌跡の補償はなされない。また、特許文献１及び２に記載の技術では、機械系の振動を考慮していないため、弾性変形の補償が加わった場合の機械振動を抑えることはできなかった。

本発明の目的は、このような問題を解決するためになされたものであり、剛性の低いベルトを力伝達手段として含む運動機構におけるベルトの弾性変形の補正を行って、軌道と軌跡とのずれ量を小さくできるモータ制御装置及び制御方法を提供することにある。

本発明の他の目的は、運動機構の振動を励起させずに、モータを高速駆動しても、軌跡精度が大きく低下しないモータ制御装置及び制御方法を提供することにある。

本発明のモータ制御装置は、位置検出部と、速度算出部と、位置制御部と、速度制御部と、トルク制御部と、補償量演算部とを備えている。位置検出部は、ベルトを力伝達手段として含む運動機構を負荷とするモータを制御するために、モータの位置（ロータの回転位置）を検出する。速度算出部は、モータの速度（ロータの回転速度）を算出する。位置制御部は、位置検出部からフィードバックされたモータの位置を示す位置フィードバックと位置指令とが一致するように速度指令を出力して位置制御をする。速度制御部は、速度算出部からフィードバックされた速度と速度指令とが一致するようにトルク指令を出力して速度制御を行う。トルク制御部は、トルク指令に基づいてトルク制御を行う。補償量演算部は、予め定めた運動機構の作用点の軌道と作用点の軌跡とのずれ量を小さくするように、位置制御部に入力される位置偏差に加算する補償量を演算する。補償量演算部は、負荷トルク推定部と、補償ゲイン記憶部と、乗算部とを備えている。負荷トルク推定部は、速度算出部が算出した速度とトルク指令とに基づいて運動機構に演算するねじれトルクを含む負荷トルクを推定する。補償ゲイン記憶部は、連続する複数の位置指令区間ごとに、ずれ量を小さくするのに必要な補償ゲインを事前の試験により定めたものを記憶する。そして乗算部は、位置指令を入力として補償ゲイン記憶部から取得した補償ゲインと負荷トルクとを乗算して補償量を演算する。

本発明では、トルク指令とモータの速度とから負荷トルクを推定する。そしてこの負荷トルクに、運動機構の作用点の予め定めた軌道に応じた弾性変形補償のための補償ゲインを乗算して補償量を求める。負荷トルクには運動機構におけるベルトの弾性変形によって生じるねじれトルクが含まれているため、この負荷トルクに補償ゲインを乗算して得た補償量は、弾性変形の補償を行うのに最適なものとなる。そこで本発明では、この補償量を位置偏差に加算することにより、弾性変形が原因となって生じる運動機構の作用点の軌道と軌跡とのずれ量を小さくする。特に本発明においては、事前の試験により複数の位置指令区間ごとに補償ゲインを定めているので、適切な補償量を得ることができて、ずれ量を適切に小さくすることができる。

なお補償量に含まれる運動機構を微細に振動させる振動成分を除去する第１のフィルタ（振動抑制フィルタ）を設けるのが好ましい。また位置指令に含まれる運動機構を微細に振動させる振動成分を除去する第２のフィルタ（プレフィルタ）を更に設けるのが好ましい。このようなフィルタを用いると、作用点の微細振動を抑制することができる。なお、第１及び第２のフィルタ（振動抑制フィルタ）は、たとえばノッチフィルタなどで構成することができる。

本発明のモータ制御方法では、連続する複数の位置指令区間ごとに、ずれ量を小さくするのに必要な補償ゲインを事前の試験により定めておく。また速度算出部が算出した速度とトルク指令とに基づいてねじれトルクを含む負荷トルクを推定する。そして位置指令に基づいて定めた位置指令区間に対応する補償ゲインと負荷トルクとを乗算して、ずれ量を小さくする補償量を演算し、補償量を位置指令とモータの位置を示すフィードバック値との位置偏差に加算する。

本発明のモータ制御装置の実施の形態の一例の構成を含むシステムの構成を示す図である。 図４に示した運動機構を駆動する場合において、補償量を位置偏差に加算しない場合のモータの速度と作用点の位置精度（先端位置精度）との関係の一例を示す図である。 補償量を位置偏差に加算した場合のモータの速度と作用点の位置精度（先端位置精度）との関係を示す図である。 モータを２台用いた多関節ロボットの運動機構の一例を示す図である。

以下図面を参照して、本発明の方法を実施する本発明のモータ制御装置の実施の形態の一例を詳細に説明する。図１は、本発明のモータ制御装置の実施の形態の一例の構成を含むシステムを示したものである。このシステムは、図４に示したモータＭ２の制御に用いられるものである。したがってモータＭ２によって駆動される機械系ＭＳは、図４のベルトＢ２、腕１０２、作用部１０３を含む運動機構である。このシステムでは、制御対象であるモータＭ２の位置を検出する位置検出部としてエンコーダＥを備えている。エンコーダＥの出力が、モータＭ２の出力軸の位置を示す位置フィードバックである。速度算出部１は、エンコーダＥの出力に基づいてモータＭ２の速度を算出するように構成されており、速度算出部１の出力が速度フィードバックとなっている。速度フィードバックが、モータＭ２の出力軸の速度を示している。位置制御部２は、位置検出部としてのエンコーダＥからフィードバックされたモータＭ２の位置を示す位置フィードバックと位置指令とが一致するように速度指令を出力して位置制御をするように構成されている。本実施の形態では、上位コントローラから与えられる位置指令をプレフィルタ（第２のフィルタ）３を通して減算手段４に与える。プレフィルタ３は、位置指令に含まれる運動機構を微細に振動させる振動成分を除去するノッチフィルタ等の振動抑制フィルタによって構成されている。減算手段４は、エンコーダＥから出力されるモータＭ２の位置を示す位置フィードバックと、フィルタ処理された位置指令との偏差をとって、位置偏差として出力する。

位置偏差は加算手段５に入力される。加算手段５は位置偏差と後述する補償量とを加算して位置制御部２に出力する。位置制御部２は、位置偏差と後述する補償量とを加算した加算値に基づいて速度指令を減算手段６に出力する。

減算手段６は、速度指令と速度算出部１が算出したモータＭ２の出力軸の回転速度との偏差を速度偏差として求めて速度制御部７に出力する。速度制御部７は、比例積分制御により、速度算出部１からフィードバックされた速度と速度指令とが一致するようにトルク指令を出力して速度制御を行う。トルク指令はトルク制御部８に入力され、トルク制御部８はトルク指令に基づいてモータＭ２のトルク制御を行う。

本実施の形態では、モータＭ２が受ける負荷トルクを推定する負荷トルク推定部として負荷トルクオブザーバ９を用いる。負荷トルクオブザーバ９は、トルク指令と速度とに基づいて摩擦トルク、ねじれトルクを含む負荷トルクを推定するように構成されている。この種のオブザーバ９の詳細は、平成２年発行の「電学論」Ｄ１１０巻１１号第１１２６頁〜第１１３２頁に示された外乱トルク推定器と実質的に同じである。

負荷トルクオブザーバ９で推定された負荷トルクは、補償量演算部１０に入力される。補償量演算部１０は、予め定めた運動機構（ＭＳ）の作用部１０３（図４）の先端（作用点）の軌道と作用点の軌跡とのずれ量を小さくするように、位置制御部２に入力される位置偏差に加算する補償量を演算する。本実施の形態の補償量演算部は、乗算部１１と補償ゲイン記憶部１２とを備えている。乗算部１１は、位置指令を入力として補償ゲイン記憶部１２から取得した補償ゲインと負荷トルクオブザーバ９が推定した負荷トルクとを乗算して補償量を演算する。本実施の形態では、補償量を振動抑制フィルタ（第１のフィルタ）１３に通して、補償量に含まれる運動機構を微細に振動させる振動成分を除去して、加算手段５に補償量を与えている。

補償ゲイン記憶部１２には、連続する複数の位置指令区間ごとに、予め定めた運動機構の作用点の軌道と作用点の軌跡とのずれ量を小さくするのに必要な補償ゲインを事前の試験により定めたものを記憶している。図２には、図４に示した運動機構を駆動する場合において、補償量を位置偏差に加算しない場合のモータＭ２の速度と作用点の位置精度（先端位置精度）との関係の一例を示しており、図３には図２と同じ条件で、補償量を位置偏差に加算した場合のモータＭ２の速度と作用点の位置精度（先端位置精度）との関係を示している。ここで作用点の位置精度（先端位置精度）とは、運動機構の作用点の軌道と軌跡とのずれ量を示すものであり、ずれが無ければ位置精度の線は直線的になる。なお図２及び図３の作用点の位置精度（先端位置精度）は、レーザ光を利用して移動する対象物の位置を測定する公知の位置測定装置を用いて作用点の軌跡を測定することにより得た。図２において、作用点の位置精度（先端位置精度）が大きく変動している原因は、運動機構中にベルトＢ２（図４）が含まれていて、このベルトＢ２が伸縮することである。ベルトＢ２の伸縮が無ければ、作用点の位置精度（先端位置精度）は殆ど変動することはない。

本実施の形態では、図３に示すように、連続する複数の位置指令区間Ｓ１〜Ｓ３ごとに、運動機構の作用点の軌道と作用点の軌跡とのずれ量を小さくするのに必要な補償ゲインＧ１〜Ｇ３を事前の試験により予め定めている。補償ゲインＧ１〜Ｇ３は、事前の試験の際に、補償量を加算しない場合の作用点の位置精度（図２）を測定し、測定結果に基づいて、補償をすべき位置指令区間Ｓ１〜Ｓ３を特定し、特定した位置指令区間Ｓ１〜Ｓ３に適した補償ゲインを決定する。図３に示すように、位置指令区間Ｓ１〜Ｓ３が長い場合には、どうしても補償精度が悪くなる。位置指令区間Ｓ１〜Ｓ３をどのように特定するか、また補償ゲインをどのような値に設定するかは、運動機構の用途に応じて定めればよい。ちなみに図３に示す作用点の位置精度が得られる補償ゲインであれば、搬送ロボットの運動機構の駆動に用いるモータの制御に十分使用可能である。

本実施の形態では、位置指令をプレフィルタ３に通し、その後位置指令と位置フィードバックとの偏差をとり、位置制御部２に通して速度指令を算出する。そして速度指令と速度との偏差をとり、速度制御部７に通し、トルク指令を算出する。このトルク指令を元に、トルク制御部８内の電力変換器を通してモータを駆動する。また、トルク指令とモータ速度から負荷トルクオブザーバ９により負荷トルクを推定し、この負荷トルクに対し、運動機構の作用点の軌道に応じた弾性変形補償のための補償ゲインを乗算し補償量を求める。この補償量を振動抑制フィルタ１３に通して位置偏差に加算する。なお、本実施の形態では、運動機構の作用点の軌道に対し、３段の補償を行ったが、さらに多くの段数の補償を行うことにより、さらに軌跡を改善できるのは勿論である。

本発明によれば、負荷トルク推定部により負荷トルクを推定し、この負荷トルクと運動機構の作用点の軌道に応じた弾性変形補償を行い、弾性変形補償による振動を励起させずに、運動機構の作用点が受ける外力による影響を抑制して、運動機構の作用点の軌跡精度を向上させることができる。

１ 速度算出部
２ 位置制御部
３ プレフィルタ（第２のフィルタ）
４ 減算手段
５ 加算手段
６ 減算手段
７ 速度制御部
８ トルク制御部
９ 負荷トルクオブザーバ（負荷トルク推定部）
１０ 補償量演算部
１１ 乗算部
１２ 補償ゲイン記憶部
１３ 振動抑制フィルタ（第１のフィルタ）
１０２ 腕
１０３ 作用部
Ｍ２ モータ
ＭＳ 機械系
Ｂ２ ベルト
Ｍ モータ
Ｅ エンコーダ
Ｓ１〜Ｓ３ 位置指令区間
Ｇ１〜Ｇ３ 補償ゲイン

Claims (6)

1. ベルトを力伝達手段として含む運動機構を負荷とするモータの位置を検出する位置検出部と、
   前記モータの速度を算出する速度算出部と、
   前記位置検出部からフィードバックされた前記モータの位置を示す位置フィードバックと位置指令とが一致するように速度指令を出力して位置制御をする位置制御部と、
   前記速度算出部からフィードバックされた前記速度と前記速度指令とが一致するようにトルク指令を出力して速度制御を行う速度制御部と、
   前記トルク指令に基づいてトルク制御を行うトルク制御部と、
   予め定めた前記運動機構の作用点の軌道と前記作用点の軌跡とのずれ量を小さくするように、前記位置制御部に入力される前記位置指令と前記位置フィードバックとの位置偏差に加算する補償量を演算する補償量演算部とを備え、
   前記補償量演算部は、前記速度算出部が算出した速度と前記トルク指令とに基づいて前記運動機構に演算するねじれトルクを含む負荷トルクを推定する負荷トルク推定部と、
   連続する複数の位置指令区間ごとに、前記ずれ量を小さくするのに必要な補償ゲインを事前の試験により定めたものを記憶する補償ゲイン記憶部と、
   前記位置指令を入力として前記補償ゲイン記憶部から取得した前記補償ゲインと前記負荷トルクとを乗算して前記補償量を演算する乗算部とを備えているモータ制御装置。
2. 前記補償量に含まれる前記運動機構を微細に振動させる振動成分を除去する第１のフィルタを更に備えている請求項１に記載のモータ制御装置。
3. 前記位置指令に含まれる前記運動機構を微細に振動させる振動成分を除去する第２のフィルタを更に備えている請求項１または２に記載のモータ制御装置。
4. ベルトを力伝達手段として含む運動機構を負荷とするモータの位置を検出する位置検出部と、
   前記モータの速度を算出する速度算出部と、
   前記位置検出部からフィードバックされた前記モータの位置を示す位置フィードバックと位置指令とが一致するように速度指令を出力して位置制御をする位置制御部と、
   前記速度算出部からフィードバックされた前記速度と前記速度指令とが一致するようにトルク指令を出力して速度制御を行う速度制御部と、
   前記トルク指令に基づいてトルク制御を行うトルク制御部とを備えたモータ制御装置を用いて、予め定めた前記運動機構の作用点の軌道と前記作用点の軌跡とのずれ量を小さくするモータ制御方法であって、
   連続する複数の位置指令区間ごとに、前記ずれ量を小さくするのに必要な補償ゲインを事前の試験により定めておき、
   前記速度算出部が算出した速度と前記トルク指令とに基づいて前記運動機構に演算するねじれトルクを含む負荷トルクを推定し、
   前記位置指令に基づいて定めた前記位置指令区間に対応する前記補償ゲインと前記負荷トルクとを乗算して、前記ずれ量を小さくする補償量を演算し、前記補償量を前記位置指令と前記位置フィードバックとの位置偏差に加算することを特徴とするモータ制御方法。
5. 前記補償量に含まれる前記運動機構を微細に振動させる振動成分を除去する請求項４に記載のモータ制御方法。
6. 前記位置指令に含まれる前記運動機構を微細に振動させる振動成分を除去する請求項４または５に記載のモータ制御方法。

Images