JP2016005296A - モータ駆動装置の制御方法、モータ駆動装置及びロボット装置 - Google Patents

Abstract

【課題】駆動時の制御モードを切り換える際に制御指令値を大きく変化させることなく、モータの回転に大きな加速度又は減速度を発生させず振動の発生を抑制できるモータ駆動装置の制御方法及びモータ駆動装置を提供する。
【解決手段】モータ制御部８１は、モータ１０１ａへの外因負荷に基づいて生成される制御目標値１３５と、制御目標値１３５に基づいて生成されるフィードフォワード値である規範モデル値１３４と、の応答速度差による差分が所定の閾値より大きいか否かを判断し、差分が所定の閾値以上であると判断した場合に、制御目標値１３５と規範モデル値１３４との加重比で、規範モデル値１３４の加重比ｈを多段階的に低減する調整を行い、加重比ｈに基づいて制御目標値１３５と規範モデル値１３４とを加重平均して制御指令値１３６として出力する。
【選択図】図４

Description

本発明は、モータを備えるモータ制御装置の制御方法、モータ制御装置及びロボット装置に関する。

従来、垂直多関節アーム及びエンドエフェクタ（以下、ロボット本体と呼ぶ）と、ロボット本体を制御する制御装置とを備えたロボット装置が普及している。このようなロボット装置を利用した部品の組立工程等において、ロボット本体の動作時の振動や各軸の動作応答の差異により、エンドエフェクタの位置精度が低下することがある。ロボット装置のエンドエフェクタの位置精度の低下は、工程の停止や組み付け誤差を発生させ工程スループットを低下させる要因となっている。また、部品の嵌合や倣い動作等の作業工程において、ロボット装置のエンドエフェクタの位置精度向上と共に、エンドエフェクタの精密な力制御が求められている。

このようなロボット装置において、駆動時の制御モードとして、例えば、位置制御モードと力制御モードとを択一的に切り換えて駆動可能なものが開発されている（特許文献１参照）。このロボット装置によれば、複数の制御モードを簡易な構成で切り換えることができるようになる。

特開平１０−５８２号公報

しかしながら、特許文献１に記載されたロボット装置では、制御モードが例えば位置制御モードと力制御モードとで択一的に切り換えられるので、切換時においては指令値が大きく変化することがある。その場合、ロボット本体等に大きな加速度又は減速度が発生し、振動を生じてしまう可能性があるという問題があった。また、このような問題は、ロボット装置のみに限られず、モータにより駆動するモータ駆動装置の全般で発生する可能性がある。

本発明は、駆動時の制御モードを切り換える際に制御指令値を大きく変化させることなく、モータの回転に大きな加速度又は減速度を発生させず振動の発生を抑制できるモータ駆動装置の制御方法、モータ駆動装置、ロボット装置を提供することを目的とする。

本発明は、モータと、前記モータに制御指令値を出力することにより前記モータの回転を制御する制御部と、を備えるモータ駆動装置の制御方法において、前記制御部が、前記モータへの外因負荷に基づいて生成される制御目標値と、前記制御目標値に基づいて生成されるフィードフォワード値である規範モデル値と、の応答速度差による差分が所定の閾値より大きいか否かを判断する判断工程と、前記制御部が、前記差分が所定の閾値以上であると判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に低減する調整を行う加重比調整工程と、前記制御部が、前記加重比に基づいて前記制御目標値と前記規範モデル値とを加重平均して前記制御指令値として出力する出力工程と、を備えることを特徴とする。

また、本発明は、モータと、前記モータに制御指令値を出力することにより前記モータの回転を制御する制御部と、を備えるモータ駆動装置において、前記制御部は、前記モータへの外因負荷に基づいて生成される制御目標値と、前記制御目標値に基づいて生成されるフィードフォワード値である規範モデル値と、の応答速度差による差分が所定の閾値より大きいか否かを判断し、前記差分が所定の閾値以上であると判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に低減する調整を行い、前記加重比に基づいて前記制御目標値と前記規範モデル値とを加重平均して前記制御指令値として出力することを特徴とする。

本発明によれば、制御部が、制御目標値と規範モデル値との差分が閾値より大きいか否かを判断し、大きいと判断した場合は規範モデル値の加重比を多段階的に低減し、加重比に基づいて制御目標値と規範モデル値との加重平均を制御指令値として出力する。即ち、力制御を実行する場合は、規範モデル値が制御目標値に対して応答速度差を発生することがあり、この場合は制御目標値と規範モデル値とが大きく離れることがある。このように、差分が所定の閾値より大きい場合に、規範モデル値の加重比を多段階的に低減しているので、制御指令値が規範モデル値から制御目標値に一度に変化することがない。これにより、駆動時に位置制御モードと力制御モードとで制御モードを切り換える際に制御指令値を大きく変化させることなく、モータの回転に大きな加速度又は減速度を発生させず振動の発生を抑制できる。

本発明の実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の実施形態に係るロボット装置の概略構成を示すブロック図である。 本発明の実施形態に係るロボット装置のモータ制御部間のデータの流れを示すチャート図である。 本発明の実施形態に係るロボット装置のモータ及びモータ制御部の概略構成を示すブロック図である。 本発明の実施形態に係るロボット装置のモータ制御部が制御指令値として加重平均を出力する手順を示すフローチャートである。 本発明の実施形態に係るロボット装置が位置制御される際の（ａ）はロボット本体の位置姿勢、（ｂ）は実施例１における手先位置変位量、（ｃ）は比較例１における手先位置変位量である。 本発明の実施形態に係るロボット装置が力制御される際の（ａ）はロボット本体の位置姿勢、（ｂ）は実施例２における押し付け力、（ｃ）は比較例２における押し付け力である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

図１に示すように、ロボット装置１は、ロボット本体（モータ駆動装置）２と、ロボット本体２を制御する制御装置３と、電源４とを備えている。ロボット本体２は、複数の関節を有する６軸の垂直多関節アーム（以下、アームと呼ぶ）２０と、エンドエフェクタであるハンド２１とを備えている。本実施形態では、アーム２０として６軸の垂直多関節アームを適用しているが、軸数は用途や目的に応じて適宜変更してもよい。

ハンド２１は、アーム２０の先端リンク６７に取り付けられて支持され、アーム２０の動作により位置及び姿勢の少なくとも一自由度が調整されるようになっている。ハンド２１は、ハンド本体２４と、ハンド本体２４に対して移動可能に配設されて、ワークＷを把持可能な複数の指２３と、先端リンク６７に固定された力覚センサ２５とを備えている。アーム２０は、７つのリンク６１〜６７と、各リンク６１〜６７を揺動又は回動可能に連結する６つの関節７１〜７６とを備えている。各リンク６１〜６７としては、長さが固定されたものを採用している。但し、例えば、直動アクチュエータにより伸縮可能なリンクを採用してもよい。

図２に示すように、各関節７１〜７６は、関節機構１０１〜１０６と、モータ制御部（制御部）８１〜８６と、を備えている。本実施形態では、全ての関節７１〜７６に関節機構１０１〜１０６とモータ制御部８１〜８６とが設けられているが、これには限られず、複数の関節７１〜７６のうちの少なくとも一部の関節に設けられていればよい。

関節機構１０１〜１０６は、関節７１〜７６を駆動するモータ１０１ａ〜１０６ａと、モータ１０１ａ〜１０６ａの回転角度を検知するエンコーダ１０１ｂ〜１０６ｂとを備えている。また、関節機構１０１〜１０６は、タイミングベルトやプーリなどによって構成される不図示の動力伝達機構と、歯車等によって構成される不図示の減速機構と、モータ１０１ａ〜１０６ａの非通電時に位置姿勢を支える不図示のブレーキ機構等とを備えている。これらモータ１０１ａ〜１０６ａ及びエンコーダ１０１ｂ〜１０６ｂは、モータ制御部８１〜８６に接続され、モータ制御部８１〜８６は、制御装置３に接続されている。尚、関節７１〜７６の詳細な説明は後述する。

制御装置３は、コンピュータにより構成され、制御目標値１３５を演算して出力し、モータ制御部８１〜８６を介してモータ１０１ａ〜１０６ａを制御することによりロボット本体２を制御可能になっている。制御装置３を構成するコンピュータは、例えば、ＣＰＵ３０と、各部を制御するためのプログラムを記憶するＲＯＭ３１と、ＲＡＭ３２と、通信制御部（Ｉ／Ｆ）３３とを備えている。ＲＡＭ３２は、ティーチングペンダント５の操作による教示点や制御指令等のデータを一時的に記憶するようになっている。ＣＰＵ３０は、教示点データからロボット本体２の各軸の軌道を生成したり、生成された軌道から各軸の制御目標値１３５を生成するようになっている。即ち、制御目標値１３５は、モータ１０１ａ〜１０６ａへの外因負荷に基づいて生成され、ここでの外因負荷は、例えば力覚センサ２５により検出される。

通信制御部３３は、モータ制御部８１〜８６へ制御目標値を通知するようになっている。制御装置３とモータ制御部８１〜８６とは、シリアル通信線６により接続されており、各モータ制御部８１〜８６への制御指令情報及び各モータ制御部８１〜８６のステータス情報をシリアル通信にて双方向に授受されるようになっている。

ティーチングペンダント５は、制御装置３に対して通信線により着脱可能に接続されており、ロボット操作及び状態表示等のユーザインターフェース機能を有する。また、電源４は電力を供給し、その電力は制御装置３の電源回路３４にて、交流−直流変換した後、制御用電源電圧とモータ駆動用電源電圧を生成して電力供給線７から各モータ制御部８１〜８６へ電力供給される。

次に、ロボット本体２の駆動時における制御装置３とモータ制御部８１〜８６との間でのシリアル通信でハンドシェイクされる制御指令及び各モータ制御部８１〜８６のステータス取得シーケンスについて、図３を用いて詳細に説明する。尚、制御装置３とモータ制御部８１〜８６との間で双方向にハンドシェイクされる処理は、ロボット装置１の起動時から継続して行われる。ハンドシェイクされる対象としては、例えば、ノード情報取得、位置制御、停止、制御パラメータ更新等を含む制御コマンド及び指令実行コマンドと、各モータ制御部８１〜８６のステータス情報である。

図３に示すように、符号２０１，２０２が、それぞれ制御装置３から各モータ制御部８１〜８６へ１つの制御コマンドを送信する際のハンドシェイクとなる。ロボット本体２の起動時は、２ミリ秒毎に常時通信がなされる。制御装置３は、各軸の制御目標位置データを含む制御コマンドＣ１，１、Ｃ２，１、Ｃ３，１、Ｃ４，１、Ｃ５，１、Ｃ６，１を生成する。ここで生成される制御コマンドには、制御目標位置データ以外に各モータ制御部８１〜８６がモータ制御に使用する制御パラメータ値等の情報も含まれる。制御コマンドは、各軸情報取得、内部状態初期化、制御パラメータ値変更、各軸のモータ回転目標位置指定による位置制御、各軸のモータ回転速度指定による速度制御、各軸のトルク指定によるトルク制御、モータ停止等のコマンドを有するようにできる。また、各コマンドに付加されるパラメータ情報により、同一コマンドにおいて返信情報の切り換えや制御パラメータ変更等を組み合わせて実行することが可能である。

制御装置３の通信制御部３３から、制御コマンドＣ１，１はモータ制御部８１へ、制御コマンドＣ２，１はモータ制御部８２へ、制御コマンドＣ３，１はモータ制御部８３へ、それぞれシリアル通信にて送信される。また、制御装置３の通信制御部３３から、制御コマンドＣ４，１はモータ制御部８４へ、制御コマンドＣ５，１はモータ制御部８５へ、制御コマンドＣ６，１はモータ制御部８６へ、それぞれシリアル通信にて送信される。各モータ制御部８１〜８６では、後述する通信制御部８１ｄ〜８６ｄに設けられる不図示の受信バッファ内に、制御装置３より受信した制御コマンドを記憶する。

続いて、制御装置３は、各モータ制御部８１〜８６への制御コマンド受信完了を確認後に、指令実行コマンドＥ１を同時送信（ブロードキャスト）する。各モータ制御部８１〜８６は、後述する演算部８１ｃ〜８６ｃにより、指令実行コマンドＥ１を受信割り込み処理にて、直前に受信した制御コマンドを実行開始する処理を実行する。これにより、モータ制御部８１では制御コマンドＣ１，１を、モータ制御部８２では制御コマンドＣ２，１を、モータ制御部８３では制御コマンドＣ３，１を、各通信制御部８１ｄ〜８３ｄの受信バッファより読み出して、制御コマンドの実行処理を行う。また、モータ制御部８４では制御コマンドＣ４，１を、モータ制御部８５では制御コマンドＣ５，１を、モータ制御部８６では制御コマンドＣ６，１を、各通信制御部８４ｄ〜８６ｄの受信バッファより読み出して、制御コマンドの実行処理を行う。更に、各モータ制御部８１〜８６は、各々現在のモータ回転位置情報を含むステータス情報Ｓ１，１、Ｓ２，１、Ｓ３，１、Ｓ４，１、Ｓ５，１、Ｓ６，１を制御装置３へ送信する処理を並行して実行する。

続いて、制御装置３は各モータ制御部８１〜８６より受信したステータス情報に基づいて、次の制御指令生成処理を実行する。これにより、制御装置３は、次回各軸の制御目標位置データを含む制御コマンドＣ１，２、Ｃ２，２、Ｃ３，２、Ｃ４，２、Ｃ５，２、Ｃ６，２の生成を行う。制御装置３からの指令実行コマンドＥｉの送信は、２ミリ秒間隔で実行されている。

このように、制御装置３と各モータ制御部８１〜８６間で授受される制御コマンドＣｉ，ｊとステータス情報Ｓｉ，ｊとの通信は、指令実行コマンドＥｉの送信間で全てハンドシェイクされる。尚、添字ｉは、各モータ制御部８１〜８６の識別インデックスであり、モータ制御部８１〜８６は、順に添字ｉ=１〜６に対応する。また、添字ｊは、制御実行コマンドが送信される毎に１増加する指令実行インデックスである。

次に、各関節７１〜７６の関節機構１０１〜１０６とモータ制御部８１〜８６とについて、図４を用いて詳細に説明する。尚、ここでは各関節７１〜７６のうち代表して関節７１について説明し、他の関節７２〜７６については関節７１と同様の構成であるので、詳細な説明を省略する。

図４に示すように、関節機構１０１のモータ１０１ａは、複数の関節７１〜７６のうちの少なくとも一部の関節の駆動源として設けられ、例えばｑ軸電流及びｄ軸電流に基づくベクトル制御により制御可能な３相ブラシレスモータとしている。モータ１０１ａは、モータ制御部８１のモータドライバ８１ｂで処理される制御演算によりモータ各相のＰＷＭ出力値が設定され、モータ各相にＰＷＭ出力される。モータ１０１ａの回転軸には、モータ軸回位置を検出するエンコーダ１０１ｂの位置検出回転部が結合されている。

エンコーダ１０１ｂは、予め設定される基準位置からの変位量を、変位分解能で定義されるパルス値として検出して、検出パルス値をデジタルデータでシリアル通信にてモータ制御部８１へ位置情報として通知する。ここでのエンコーダ１０１ｂの検出分解能は１７ビット／回転であり、モータ制御部８１への位置情報の通知は１００マイクロ秒毎に行われるようになっている。

モータ制御部８１は、制御基板を備えており、該制御基板に、電源回路８１ａと、モータを駆動するモータドライバ８１ｂと、演算部８１ｃと、通信制御部（Ｉ／Ｆ）８１ｄとが実装されている。通信制御部８１ｄは、制御装置３とシリアル通信制御を行うようになっている。

演算部８１ｃは、制御目標値１３５と、制御目標値１３５に基づいて演算された規範モデル値１３４と、の差分が所定の閾値より大きいか否かを判断する。そして、演算部８１ｃは、差分が所定の閾値以上であると判断した場合に、制御目標値１３５と規範モデル値１３４とのうちで規範モデル値１３４の加重比ｈを多段階的に低減する調整を行う。更に、演算部８１ｃは、加重比ｈに基づいて制御目標値１３５と規範モデル値１３４とを加重平均して制御指令値１３６として出力するようになっている。尚、規範モデル値１３４は、制御目標値１３５に基づいて生成されるフィードフォワード値である。

また、演算部８１ｃは、差分が所定の閾値より小さいと判断した場合に、制御目標値１３５と規範モデル値１３４とのうちで規範モデル値１３４の加重比ｈを増加する調整を行うようになっている。更に、演算部８１ｃは、加重比ｈに調整係数を積算することにより加重比ｈを調整し、差分に基づいて調整係数を多段階的に設定するようになっている。

演算部８１ｃは、不図示のアナログ／デジタル変換器と、制御プログラム及び制御パラメータを記憶する揮発メモリと、制御演算時に一時的に利用する不揮発メモリ等が含まれ、１つのモータ１０１ａの駆動制御処理を実行するようになっている。また、演算部８１ｃは、フィードバック制御ブロック８１ｅと、規範モデルブロック８１ｆと、切換ブロック８１ｇと、加重比設定部８１ｈとを備えている。

フィードバック制御ブロック８１ｅは、モータ軸に結合されるエンコーダ１０１ｂから取得される位置情報に基づいてモータ軸位置を目標位置に制御するようになっている。フィードバック制御ブロック８１ｅは、位置ＰＩＤ制御１１０と、速度ＰＩＤ制御１１１と、モータトルクに寄与する電流を制御するｑ軸電流制御１１２と、モータ界磁電流を制御するｄ軸電流制御１１３とをカスケード接続した制御ブロックを備えている。位置ＰＩＤ制御１１０は、演算部８１ｃに内蔵されるタイマにて２ミリ秒周期で処理されており、モータ回転位置制御目標値とモータ軸回転位置との位置偏差情報に基づいて比例／積分／微分制御を行う所謂ＰＩＤ制御器である。これにより、位置ＰＩＤ制御１１０は、モータ回転速度制御目標値１１４を出力する。ここで、位置ＰＩＤ制御１１０にて処理されるＰＩＤ制御演算の比例／積分／微分制御各係数値は、モータ制御部８１〜８６ごとに個別に設定される。ここでの設定は、例えば、モータ制御部８１にて駆動するモータ１０１ａ及びモータ軸に接続される減速機、リンク負荷、機械特性及び制御帯域、制御安定性を考慮して設定される。

速度ＰＩＤ制御１１１は、演算部８１ｃに内蔵されるタイマにて０．５ミリ秒周期で処理されている。速度ＰＩＤ制御１１１は、位置ＰＩＤ制御１１０の演算結果として出力されるモータ回転速度制御目標値１１４と周期的に取得されるモータ回転位置情報よりモータ軸回転速度との速度偏差に基づいて比例／積分／微分制御を行う所謂ＰＩＤ制御器である。これにより、速度ＰＩＤ制御１１１は、電流制御目標値１１５を出力する。ここで、速度ＰＩＤ制御１１１にて処理されるＰＩＤ制御演算の比例／積分／微分制御各係数値は、モータ制御部８１〜８６ごとに個別に設定される。ここでの設定は、例えば、モータ制御部８１にて駆動するモータ１０１ａ及びモータ軸に接続される減速機、リンク負荷、機械特性及び制御帯域、制御安定性を考慮して設定される。

速度ＰＩＤ制御１１１の処理にて出力される電流制御目標値１１５は、外乱オブザーバ１１６により補正される処理が実行される。ここでは、まず、電流制御目標値１１５にトルク定数を乗じた制御指令トルク値と、モータ回転位置情報より算出されるモータ回転加速度値に慣性モーメントを乗じたトルク値と、の差を外乱として補正値１１７を算出する。そして、速度ＰＩＤ制御１１１から出力される電流制御目標値１１５を、補正値１１７の加算により補正する。

モータ電流制御は、演算部８１ｃに内蔵されるタイマにて０．１ミリ秒周期にて処理されている。補正後の電流制御目標値１１８は、トルク制限処理１１９及び機械構成に起因する振動をカットするノッチフィルタ処理１２０で処理され、電流制御目標値１２１とされる。ｑ軸電流ＰＩＤ制御１１２は、電流制御目標値１２１と、モータ各相に流れる電流情報から算出されるｑ軸電流値と、の偏差に基づいて比例／積分／微分制御を行うようになっている。一方、ｄ軸電流ＰＩＤ制御１１３は、モータ各相に流れる電流情報から算出されるｄ軸電流がゼロとなるように比例／積分／微分制御を行う。

ｑ軸電流及びｄ軸電流は、モータ制御部８１に配設される不図示の制御基板に実装される電流検出素子及びアナログ／デジタル変換器により、３相ブラシレスモータのＵ／Ｗ相電流値をアナログ／デジタル変換した電流値を使用して、数式１で算出される。

但し、ｉｄ：ｄ軸電流［Ａ］、ｉｑ：ｑ軸電流［Ａ］、θｅ：モータ電気角［ｒａｄ］、ｉｕ：モータｕ相電流［Ａ］、ｉｗ：モータｗ相電流［Ａ］

数式１の変換式により、クラーク／パーク変換１２２にて、モータトルクに寄与する電流（ｑ軸電流）とモータ界磁電流（ｄ軸電流）が算出される。ｑ軸電流ＰＩＤ制御１１２とｄ軸電流ＰＩＤ制御１１３の演算結果は、ｄｑ軸非干渉化器１２３にて、数式２の補正式で演算される。

但し、ωｅ：モータ電気角速度［ｒａｄ／ｓ］、Ｌｄ：ｄ軸インダクタンス［Ｈ］、Ｌｑ：ｑ軸インダクタンス［Ｈ］、φ：鎖交磁束［ｗｂ］

これにより、ｖｄ^＊（符号１２４）とｖｑ^＊（符号１２５）とを算出し、非干渉化したｑ軸電圧１２６及びｄ軸電圧１２７を算出することができる。更に、ｑ軸電圧１２６及びｄ軸電圧１２７は、数式３の変換式により、逆クラーク／パーク変換１２８にてモータ各相出力値を決定する。モータ各相出力値は、ＰＷＭ波形生成１２９により、ＰＷＭ波形に変換された後にモータドライバ８１ｂを介して出力される。

但し、Ｅｕ：モータＵ相出力値［Ｖ］、Ｅｖ：モータＶ相出力値［Ｖ］、Ｅｗ：モータＷ相出力値［Ｖ］、ｖｄ：ｄ軸電圧［Ｖ］ｖｑ：ｑ軸電圧［Ｖ］

規範モデルブロック８１ｆは、モータ制御処理とモータ機械挙動をモデル化するものであり、フィードバック制御処理を疑似した位置制御伝達関数１３０と、速度制御伝達関数１３１と、モータ機械挙動をモデル化した伝達関数１３２とを備えている。位置制御伝達関数１３０は、モータ機械挙動をモデル化した伝達関数１３２の出力値を積分器１３３にて規範モデル値１３４を、制御装置３から送信される制御目標値１３５に、制御する位置ＰＩＤ制御をモデル化したものである。これは、数式４の伝達関数で定義される。

但し、Ｋｐ_ｐｏｓ：比例係数、Ｋｉ_ｐｏｓ：積分係数、Ｋｄ_ｐｏｓ：微分係数、ｓ：ラプラス演算子

比例係数Ｋｐ_ｐｏｓは、モータ制御部８１〜８６にて駆動するモータ１０１ａ〜１０６ａ及びモータ軸に接続される減速機、リンク負荷特性、規範とする制御帯域、制御安定性を考慮した値をモータ制御部８１〜８６ごとに設定される。積分係数Ｋｉ_ｐｏｓと、微分係数Ｋｄ_ｐｏｓも同様である。位置制御伝達関数１３０の出力値は、規範モデル速度制御目標値及びフィードバック制御ブロック８１ｅにおける速度ＰＩＤ制御１１１のフィードフォワード信号としてモータ回転速度目標値に加算される。

速度制御伝達関数１３１は、速度ＰＩＤ制御をモデル化したものであり、数式５の伝達関数で定義される。

但し、Ｋｐ_ｖｅｌ：比例係数、Ｋｉ_ｖｅｌ：積分係数、Ｋｄ_ｖｅｌ：微分係数

比例係数Ｋｐ_ｖｅｌは、モータ制御部８１〜８６にて駆動するモータ１０１ａ〜１０６ａ及びモータ軸に接続される減速機、リンク負荷特性、規範とする制御帯域、制御安定性を考慮した値をモータ制御部８１〜８６ごとに設定される。積分係数Ｋｉ_ｖｅｌと、微分係数Ｋｄ_ｖｅｌも同様である。速度制御伝達関数１３１の出力値は、モータ機械挙動をモデル化した伝達関数１３２のモータ発生トルク値及びフィードバック制御ブロック８１ｅにおけるｑ軸電流ＰＩＤ制御１１２のフィードフォワード信号として電流制御目標値に加算される。

モータ機械挙動をモデル化した伝達関数１３２は、モータロータをモデル化したものであり、数式６の伝達関数で定義される。

但し、Ｊ：ロータイナーシャ、Ｄ：粘性制動係数

ロータイナーシャＪ及び粘性制動係数Ｄは、各モータ制御部８１〜８６にて駆動するモータ１０１ａ〜１０６ａの機械特性値及びモータ軸に接続される減速機、リンク負荷等の特性値を考慮して設定する。モータ機械挙動をモデル化した伝達関数１３２から出力される規範モデル速度を積分器１３３により積分処理した後、制御規範となるモータロータ回転位置とするようになっている。

次に、本発明の特徴である切換ブロック８１ｇ及び加重比設定部８１ｈについて、説明する。切換ブロック８１ｇ及び加重比設定部８１ｈで実行される処理は、フィードバック制御ブロック８１ｅの位置ＰＩＤ制御１１０の処理周期である２ミリ秒周期で実行される。

加重比設定部８１ｈは、制御装置３から受信する制御目標値１３５と規範モデルブロック８１ｆから出力される規範モデル値１３４の正負を除去した差分を演算するようになっている。そして、加重比設定部８１ｈは、差分に基づいて、制御装置３から受信する制御目標値１３５と規範モデルブロック８１ｆから出力される規範モデル値１３４との各加重比を決定する処理を実行するようになっている。

尚、規範モデル値１３４の加重比をｈとし、制御目標値１３５の加重比を１−ｈとしている。また、加重比ｈは０より大きく１以下の値で設定され、ロボット装置１の起動時の初期値は１としている。

切換ブロック８１ｇは、加重比設定部８１ｈで決定された制御目標値１３５と規範モデル値１３４との各加重比に基づいて、制御目標値１３５と規範モデル値１３４の加重平均値を出力するようになっている。出力された加重平均値は、フィードバック制御ブロック８１ｅの制御指令値１３６として位置ＰＩＤ制御１１０に入力されるようになっている。

上述したロボット装置１の制御方法の手順について、図５に示すフローチャートに沿って説明する。

加重比設定部８１ｈは、制御目標値１３５と規範モデル値１３４とを取得し（ステップＳ１）、その正負を除去した差分を演算する（ステップＳ２）。そして、加重比設定部８１ｈは、得られた差分が予め設定した所定の閾値以上であるか否かを判断する（ステップＳ３、判断工程）。所定の閾値は、ロボット本体２のシステムの機械特性や、ハンド２１及び把持するワーク重量等に基づいて決定される値であり、ロボット装置１で実施される作業内容に応じて適宜設定することができる。本実施形態では、例えば、エンコーダパルス値５００［ＰＵＬＳＥ］（０．０２［ｒａｄ］）とし、全てのモータ制御部８１〜８６に同値を設定している。

加重比設定部８１ｈは、得られた差分が予め設定した所定の閾値以上であると判断した場合は、差分に基づいて調整係数αを処理の度に多段階的に設定する（ステップＳ４、調整係数設定工程）。調整係数αは、加重比調整工程における１ステップで変更される制限係数であり、ロボット本体２のシステムの機械特性や、ロボット装置１が行う作業工程や、ロボット本体２の作業姿勢等に応じて設定される。本実施形態では、１回目の処理において、例えばα＝０．７とする。

加重比設定部８１ｈは、規範モデル値１３４の加重比ｈに調整係数αを積算して調整し、その値を新たな加重比ｈとする（ステップＳ５、加重比調整工程）。このため、制御目標値１３５と規範モデル値１３４との差分が閾値より大きい場合は、規範モデル値１３４の加重比ｈを下げるようにしている。

そして、加重比設定部８１ｈは、切換ブロック８１ｇに対して加重比ｈ，ｈ−１を設定する（ステップＳ６）。切換ブロック８１ｇは、制御目標値１３５に対しては加重比ｈ−１を積算し、規範モデル値１３４に対しては加重比ｈを積算し、これらの加重平均を演算し、制御指令値１３６として出力する（ステップＳ７、出力工程）。

一方、加重比設定部８１ｈは、得られた差分が予め設定した所定の閾値以上ではないと判断した場合は、差分に基づいて調整係数βを処理の度に多段階的に設定する（ステップＳ８、調整係数設定工程）。調整係数βは、加重比調整工程における１ステップで変更される制限係数であり、ロボット本体２のシステムの機械特性や、ロボット装置１が行う作業工程や、ロボット本体２の作業姿勢等に応じて設定される。本実施形態では、１回目の処理において、例えばβ＝１．４とする。

加重比設定部８１ｈは、規範モデル値１３４の加重比ｈに予め設定した調整係数βを積算して調整し、その値を新たな加重比ｈとする（ステップＳ９、加重比調整工程）。このため、制御目標値１３５と規範モデル値１３４との差分が閾値より小さい場合は、規範モデル値１３４の加重比ｈを上げるようにしている。

そして、加重比設定部８１ｈは、加重比ｈが１を超えているか否かを判断する（ステップＳ１０）。加重比設定部８１ｈが、加重比ｈは１を超えていると判断した場合は、加重比ｈを１に設定し（ステップＳ１１）、加重比ｈが１を超えないようにする。ステップＳ９の後、あるいは加重比設定部８１ｈが加重比ｈは１を超えていないと判断した場合は、加重比設定部８１ｈは、上述と同様に加重比ｈ，ｈ−１を設定し（ステップＳ６）、加重平均を制御指令値１３６として出力する（ステップＳ７、出力工程）。

上述したように本実施形態のロボット装置１によれば、モータ制御部８１〜８６が、制御目標値１３５と規範モデル値１３４との差分が所定の閾値より大きいか否かを判断する。そして、モータ制御部８１〜８６が、差分は閾値より大きいと判断した場合は規範モデル値１３４の加重比ｈを多段階的に低減し、加重比ｈに基づいて制御目標値１３５と規範モデル値１３４との加重平均を制御指令値１３６として出力する。また、モータ制御部８１〜８６が、差分は閾値より大きくないと判断した場合は規範モデル値１３４の加重比ｈを多段階的に増加し、加重比ｈに基づいて制御目標値１３５と規範モデル値１３４との加重平均を制御指令値１３６として出力する。

即ち、力制御を実行する場合は、規範モデル値１３４が制御目標値１３５に対して応答速度差を発生することがあり、この場合は制御目標値１３５と規範モデル値１３４とが大きく離れることがある。このように、差分が所定の閾値より大きい場合に、規範モデル値１３４の加重比ｈを多段階的に低減しているので、制御指令値１３６が規範モデル値１３４から制御目標値１３５に一度に変化することがない。これにより、駆動時に位置制御モードと力制御モードとで制御モードを切り換える際に制御指令値１３６を大きく変化させることなく、モータ１０１ａ〜１０６ａの回転に大きな加速度又は減速度を発生させず振動の発生を抑制できる。

また、本実施形態のロボット装置１によれば、モータ制御部８１〜８６が、規範モデル値１３４と制御目標値１３５との差分に基づいて調整係数α，βを多段階的に設定する。このため、加重比ｈ，ｈ−１の変化を円滑に行うことができ、モータ１０１ａ〜１０６ａの回転に大きな加速度又は減速度を発生させず振動の発生をより抑制できる。

尚、上述した実施形態では、判断工程、調整係数設定工程、加重比調整工程、出力工程の全てをモータ制御部８１〜８６により実行した場合について説明したが、これには限定されない。例えば、これらの工程の一部又は全部を制御装置３により実行するようにしてもよい。この場合、例えば、制御装置３と各モータ制御部８１〜８６との間のシリアル通信で受信される各モータ制御部８１〜８６のステータス情報に基づいて、各モータ制御部８１〜８６の加重比ｈを決定する。そして、制御装置３は、制御コマンドの付加情報として加重係数値を含んだ制御コマンドを各モータ制御部８１〜８６に送信することにより上述の各工程を実行するようにできる。

また、上述した実施形態では、加重比設定部８１ｈは、調整係数α，βを処理の度に多段階的に設定する場合について説明したが、これには限定されず、例えば、調整係数α，βを一定に設定してもよい。この場合、調整係数設定工程が不要になるので、処理を簡素化することができる。

また、上述した実施形態では、モータ駆動装置としてロボット装置１を適用した場合について説明したが、これには限定されない。即ち、本発明は、モータにより駆動されるモータ駆動装置の全般に適用することができる。

また、以上述べた本実施形態の各工程の処理動作は、具体的には演算部８１ｃ〜８６ｃのシーケンシャル動作により実行される場合について説明したが、これには限定されない。例えば、上述した工程を有し機能を実現するソフトウェアの制御プログラムを記録した記録媒体をＣＰＵに供給し、ＣＰＵが記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した各実施の形態の機能を実現することになり、ロボット制御プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。プログラムを供給するための記録媒体としては、例えばＲＯＭや、ＨＤＤ、外部記憶装置、記録ディスク等を用いてもよい。

工場のラインに設置されたロボット装置１により所定の動作を行わせ、ハンド２１の手先位置精度と力制御時の力精度とを測定した。

（実施例１）
手先位置精度の計測は、図１に示すロボット本体２を用い、ハンド２１には手先位置精度計測用のワークＷを把持させた。そして、図６（ａ）に示すように、ワークＷをＸ軸方向へ４５ミリメートル移動させ、ワークＷのＹ軸及びＺ軸方向位置の変位量を不図示のレーザ変位計にて定点観測した。尚、ここで使用したレーザ変位計は、ＫＥＹＥＮＣＥ製ＬＫ−Ｈ５７であり、０．１ミリ秒周期で計測してデータの取り込みを行った。その結果を、図６（ｂ）に示す。

（比較例１）
上述した本実施形態のロボット装置１を用い、加重比ｈを１に設定して、上述と同様の測定を行った。その結果を、図６（ｃ）に示す。

（実施例２）
力制御時の力精度の計測は、図１に示すロボット本体２を用い、ハンド２１には手先位置精度計測用のワークＷを把持させた。そして、図７（ａ）に示すように、ワークＷを−Ｚ軸方向に動作させ、作業台１１へＦ１＝１０ニュートンの力で５秒間押し付けた。ハンド２１に設けられた力覚センサ２５により、動作中の押し付け力を２ミリ秒周期で計測した。その結果を、図７（ｂ）に示す。

（比較例２）
上述した本実施形態のロボット装置１を用い、加重比ｈを１に設定して、上述と同様の測定を行った。その結果を、図７（ｃ）に示す。

これらの結果から明らかなように、本実施形態のロボット装置１によれば、規範モデル値１３４を制御指令値１３６として利用する場合に比べて、以下のような効果が明らかになった。即ち、本実施形態のロボット装置１によれば、位置制御時には手先位置の精度を低下させることがなく、かつ力制御では押し付け力を目標値に小さい振幅で押し付けることができた。このため、力制御時のロボット本体２における振動の発生を抑制することが確認された。

１…ロボット装置、２…ロボット本体（モータ駆動装置）、３…制御装置、２０…多関節アーム、７１〜７６…関節、８１〜８６…モータ制御部（制御部）、１０１ａ〜１０６ａ…モータ、１３４…規範モデル値、１３５…制御目標値、１３６…制御指令値、ｈ…規範モデル値の加重比、α，β…調整係数

Claims (8)

1. モータと、前記モータに制御指令値を出力することにより前記モータの回転を制御する制御部と、を備えるモータ駆動装置の制御方法において、
   前記制御部が、前記モータへの外因負荷に基づいて生成される制御目標値と、前記制御目標値に基づいて生成されるフィードフォワード値である規範モデル値と、の応答速度差による差分が所定の閾値より大きいか否かを判断する判断工程と、
   前記制御部が、前記差分が所定の閾値以上であると判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に低減する調整を行う加重比調整工程と、
   前記制御部が、前記加重比に基づいて前記制御目標値と前記規範モデル値とを加重平均して前記制御指令値として出力する出力工程と、を備える、
   ことを特徴とするモータ駆動装置の制御方法。
2. 前記加重比調整工程では、前記制御部が、前記差分が所定の閾値より小さいと判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に増加する調整を行う、
   ことを特徴とする請求項１記載のモータ駆動装置の制御方法。
3. 前記加重比調整工程では、前記制御部が、前記加重比に調整係数を積算することにより前記加重比を調整し、
   前記制御部が、前記調整係数を設定する調整係数設定工程を備える、
   ことを特徴とする請求項１又は２に記載のモータ駆動装置の制御方法。
4. 前記調整係数設定工程では、前記制御部が、前記判断工程で演算された前記差分に基づいて前記調整係数を多段階的に設定する、
   ことを特徴とする請求項３記載のモータ駆動装置の制御方法。
5. 請求項１乃至４のいずれか１項に記載のモータ駆動装置の制御方法の各工程をコンピュータに実行させるためのプログラム。
6. 請求項５に記載のプログラムが記録されたコンピュータが読み取り可能な記録媒体。
7. モータと、前記モータに制御指令値を出力することにより前記モータの回転を制御する制御部と、を備えるモータ駆動装置において、
   前記制御部は、前記モータへの外因負荷に基づいて生成される制御目標値と、前記制御目標値に基づいて生成されるフィードフォワード値である規範モデル値と、の応答速度差による差分が所定の閾値より大きいか否かを判断し、前記差分が所定の閾値以上であると判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に低減する調整を行い、前記加重比に基づいて前記制御目標値と前記規範モデル値とを加重平均して前記制御指令値として出力する、
   ことを特徴とするモータ駆動装置。
8. 複数の関節を含む多関節アームを有するロボット本体と、前記複数の関節のうちの少なくとも一部の関節の駆動源として設けられるモータと、前記モータに制御指令値を出力することにより前記モータの回転を制御する制御部と、を備えるロボット装置において、
   前記制御部は、前記モータへの外因負荷に基づいて生成される制御目標値と、前記制御目標値に基づいて生成されるフィードフォワード値である規範モデル値と、の応答速度差による差分が所定の閾値より大きいか否かを判断し、前記差分が所定の閾値以上であると判断した場合に、前記制御目標値と前記規範モデル値との加重比で、前記規範モデル値の加重比を多段階的に低減する調整を行い、前記加重比に基づいて前記制御目標値と前記規範モデル値とを加重平均して前記制御指令値として出力する、
   ことを特徴とするロボット装置。

Images