特開 2025-17182 ロボット駆動装置、ロボット駆動方法及びロボット駆動プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2025017182

(43)【公開日】2025-02-05

(54)【発明の名称】ロボット駆動装置、ロボット駆動方法及びロボット駆動プログラム

(51)【国際特許分類】

   B25J 13/08 20060101AFI20250129BHJP

【ＦＩ】

B25J13/08 Z

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2023120133

(22)【出願日】2023-07-24

(71)【出願人】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】100103894

【弁理士】

【氏名又は名称】家入 健

(72)【発明者】

【氏名】羽多野 顕

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707CX01

3C707HS27

3C707KS35

3C707KW05

3C707LT11

3C707LV23

3C707LW05

(57)【要約】

【課題】従来のロボット駆動装置では、位置制御及びトルク制御を十分に安定化させることが困難である問題があった。
【解決手段】本発明のロボット駆動装置は、ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、前記制御指令値を生成する制御指令値生成部と、制御指令値生成部の周期処理と同期した周期で、位置目標値及び仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成回路に出力する補正処理部と、を有し、前記補正処理部において、モータに与えられる外乱のうち周波数が低い外乱に対しては位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い外乱に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用する。
【選択図】図５

【特許請求の範囲】

【請求項1】

ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、
少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、
前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有し、
前記補正処理部は、
前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行うロボット駆動装置。

【請求項2】

前記補正処理部は、
予め設定される値であって、前記モータに与えられる外乱により変動する外乱指標の目標値である外乱指標目標値を参照する参照処理と、
前記外乱指標の成分うち低周波側の周波数特性を有する前記低周波側外乱成分が前記外乱指標目標値以上であれば、予め設定された算出式に基づき算出される値を前記補正後位置目標値に設定し、前記低周波側外乱成分が前記外乱指標目標値よりも小さければ前記補正後位置目標値に予め設定される初期値を設定する第１の補正処理と、
前記外乱指標の成分うち高周波側の周波数特性を有する前記高周波側外乱成分が前記外乱指標目標値以上であれば、予め設定された算出式に基づき算出される値を前記補正後仮想バネ定数に設定し、前記低周波側外乱成分が前記外乱指標目標値よりも小さければ予め設定される初期値を前記補正後仮想バネ定数に設定する第２の補正処理と、を行う請求項１に記載のロボット駆動装置。

【請求項3】

前記補正処理部は、
前記第１の補正処理において補正された前記位置目標値を参照して前記第２の補正処理を行う請求項２に記載のロボット駆動装置。

【請求項4】

前記補正処理部は、
前記低周波側外乱成分が前記高周波側外乱成分以上であった場合、前記第１の補正処理に加えて、予め設定される初期値を前記補正後仮想バネ定数に設定する第１の補足補正処理を行い、
前記低周波側外乱成分が前記高周波側外乱成分よりも小さかった場合、前記第２の補正処理に加えて、予め設定される初期値を前記補正後位置目標値に設定する第２の補足補正処理を行う請求項２に記載のロボット駆動装置。

【請求項5】

前記外乱指標は、前記関節の位置偏差又は前記モータのトルク偏差であり、
前記補正処理部は、前記関節の位置偏差又は前記モータのトルク偏差を高周波側と低周波側の成分に分離するフィルタ処理部を有する請求項２に記載のロボット駆動装置。

【請求項6】

ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、
少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、
前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有するロボット駆動装置におけるロボット駆動方法であって、
前記補正処理部において、
前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行うロボット駆動方法。

【請求項7】

ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、
少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、
前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有し、前記補正処理部における演算処理を実行する演算部を有するロボット駆動装置において、前記演算部で実行されるロボット駆動プログラムであって、
前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行うロボット駆動プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明はロボット駆動装置、ロボット駆動方法及びロボット駆動プログラムに関し、例えば、仮想バネ定数と位置目標を用いたロボットの制御を行うロボット駆動装置、ロボット駆動方法及びロボット駆動プログラムに関する。

【背景技術】

【0002】

物体に接触させるアームを有するロボットでは、アームが物体に接触或いは衝突する際にトルクの入力変化量が大きく、かつ、変化の周波数が高いため制御が不安定になりやすい問題がある。そこで、このような不安定な制御を安定化させる技術が特許文献１に開示されている。

【0003】

特許文献１に記載のロボットは、本体部と、床面上を移動する移動機構をそれぞれ含む、複数の移動機構部と、１又は複数の関節部をそれぞれ備え、前記本体部と各前記移動機構部との間をそれぞれ結合する、複数の脚部と、を備えたロボットであって、前記関節部は、インピーダンス制御により駆動される駆動軸を含む。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０２２－０９９０３４号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかしながら、特許文献１に記載された位置制御においては、位置制御の精度を保証するために仮想バネ定数を比較的高めに設定する必要があり、現在位置の算出精度が低いと目標位置を変化させることが困難で振動しやすい問題がある。また、バネ定数制御においては、仮想バネ定数の取りうる値に下限が存在し、位置偏差を許容できなくなるといった課題がある。このように特許文献１に記載のロボットでは、位置制御及びトルク制御を安定化させるために取得する制御パラメータに利用の制限があるため、位置制御及びトルク制御を十分に安定化させることが困難である問題がある。

【0006】

本発明は、このような問題を解決するためになされたものであり、ロボットのアーム等の駆動部における位置制御をより安定化させることを目的とするものである。

【課題を解決するための手段】

【0007】

本発明にかかるロボット駆動装置の一態様は、ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有し、前記補正処理部は、前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行う。

【0008】

本発明にかかるロボット駆動方法の一態様は、ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有するロボット駆動装置におけるロボット駆動方法であって、前記補正処理部において、前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行う。

【0009】

本発明にかかるロボット駆動プログラムの一態様は、ロボットの関節に取り付けられたモータに与える駆動電流を制御指令値に応じて増減させるとともに、前記モータの回転角に対応する位置現在値を出力するモータ駆動回路と、少なくとも、前記位置現在値、前記ロボットの関節の位置目標値、仮想バネ定数、トルク目標値及び速度目標値を用いて前記制御指令値を生成する制御指令値生成部と、前記制御指令値生成部の周期処理と同期した周期で、前記位置目標値及び前記仮想バネ定数を補正した補正後位置目標値及び補正後仮想バネ定数を前記制御指令値生成部に出力する補正処理部と、を有し、前記補正処理部における演算処理を実行する演算部を有するロボット駆動装置において、前記モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては前記位置目標値に対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数に補正を加える仮想バネ定数制御手法を適用した補正処理を行う。

【0010】

本発明にかかるロボット駆動装置、ロボット駆動方法及びロボット駆動プログラムでは、モータに与えられる外乱の周波数が低い場合には位置目標値に対して補正を加え、外乱の周波数が高い場合には仮想バネ定数に補正を加える。

【発明の効果】

【0011】

本発明により、ロボットの位置制御の安定度を高めることができる。

【図面の簡単な説明】

【0012】

【図1】実施の形態１にかかるロボット駆動装置のブロック図である。

【図2】位置目標値を変化させる場合の制御ロジックを説明する図である。

【図3】仮想バネ定数を変化させる場合の制御ロジックを説明する図である。

【図4】実施の形態１にかかる補正処理部のブロック図である。

【図5】実施の形態１にかかる補正処理部の動作を説明するフローチャートである。

【図6】実施の形態２にかかる補正処理部のブロック図である。

【図7】実施の形態２にかかる補正処理部の動作を説明するフローチャートである。

【発明を実施するための形態】

【0013】

説明の明確化のため、以下の記載及び図面は、適宜、省略、及び簡略化がなされている。また、様々な処理を行う機能ブロックとして図面に記載される各要素は、ハードウェア的には、ＣＰＵ（Central Processing Unit）、メモリ、その他の回路で構成することができ、ソフトウェア的には、メモリにロードされたプログラムなどによって実現される。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、またはそれらの組合せによっていろいろな形で実現できることは当業者には理解されるところであり、いずれかに限定されるものではない。なお、各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。

【0014】

また、上述したプログラムは、コンピュータに読み込まれた場合に、実施形態で説明された１又はそれ以上の機能をコンピュータに行わせるための命令群（又はソフトウェアコード）を含む。プログラムは、非一時的なコンピュータ可読媒体又は実体のある記憶媒体に格納されてもよい。限定ではなく例として、コンピュータ可読媒体又は実体のある記憶媒体は、random-access memory（RAM）、read-only memory（ROM）、フラッシュメモリ、solid-state drive（SSD）又はその他のメモリ技術、CD-ROM、digital versatile disc（DVD）、Blu-ray（登録商標）ディスク又はその他の光ディスクストレージ、磁気カセット、磁気テープ、磁気ディスクストレージ又はその他の磁気ストレージデバイスを含む。プログラムは、一時的なコンピュータ可読媒体又は通信媒体上で送信されてもよい。限定ではなく例として、一時的なコンピュータ可読媒体又は通信媒体は、電気的、光学的、音響的、またはその他の形式の伝搬信号を含む。

【0015】

実施の形態１
図１に実施の形態１にかかるロボット駆動装置１のブロック図を示す。図１に示す例は、ロボットのアームの関節等に設けられたモータを駆動することでロボットを動作させるものである。図１に示すように、実施の形態１にかかるロボット駆動装置１は、補正処理部１０、制御指令値生成部１１、モータ駆動回路１２を有する。なお、ロボット駆動装置１では、位置目標値Ｐｏｓ＿ｒｅｆ、仮想バネ定数Ｋｐ＿ｒｅｆ、トルク目標値τｒｅｆ、速度目標値Ｖｅｌ＿ｒｅｆ及び制御ゲインＫｄ＿ｒｅｆが図示しない上位システムから与えられる。また、ロボット駆動装置１では、制御ゲインＫｄ＿ｒｅｆが与えられない場合もある。

【0016】

モータ駆動回路１２は、ロボットの関節に取り付けられたモータに与える駆動電流Ｉ＿ｏｄを制御指令値に応じて増減させるとともに、モータの回転角に対応する位置現在値Ｐｏｓ_ｃｕｒを出力する。制御指令値生成部１１は、補正処理部１０は、少なくとも、位置現在値Ｐｏｓ＿ｃｕｒ、ロボットの関節の位置目標値Ｐｏｓ＿ｒｅｆ、仮想バネ定数Ｋｐ＿ｒｅｆ、トルク目標値τｒｅｆ及び速度目標値Ｖｅｌ＿ｒｅｆを用いて制御指令値を生成する。補正処理部１０は、制御指令値生成部１１の周期処理と同期した周期で、位置目標値Ｐｏｓ＿ｒｅｆ及び仮想バネ定数Ｋｐ＿ｒｅｆを補正した補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ及び補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを出力する。なお、実施の形態１では、制御指令値生成部１１は、補正処理部１０が出力する補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ及び補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを位置目標値Ｐｏｓ＿ｒｅｆ及び仮想バネ定数Ｋｐ＿ｒｅｆとして利用する。

【0017】

図１では、制御指令値生成部１１の構成の一例を示している。具体的には、図１では、制御指令値生成部１１が減算器２１、乗算器２２、現在速度算出部２３、減算器２４、乗算器２５、加算器２６、加算器２７を有する例を示した。

【0018】

減算器２１は、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃから位置現在値Ｐｏｓ＿ｃｕｒを減算して位置偏差を出力する。乗算器２２は、減算器２１が出力する位置偏差に補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを乗算した値を出力する。現在速度算出部２３は、位置現在値Ｐｏｓ＿ｃｕｒを微分して速度現在値を出力する。減算器２４は、速度目標値Ｖｅｌ＿ｒｅｆから速度現在値を減算して速度偏差を出力する。乗算器２５は、減算器２４が出力する速度偏差に制御ゲインＫｄ＿ｒｅｆを乗算した値を出力する。加算器２６は、乗算器２５が出力した値にトルク目標値τｒｅｆを加算した値を出力する。加算器２７は、乗算器２２が出力した値に加算器２７が出力した値を加算してモータ駆動回路１２に与える制御指令値を出力する。

【0019】

実施の形態１にかかるロボット駆動装置１では、補正処理部１０において、モータに与えられる外乱のうち周波数が低い低周波側外乱成分に対しては位置目標値Ｐｏｓ＿ｒｅｆに対して補正を加える位置制御手法を適用し、周波数が高い高周波側外乱成分に対しては仮想バネ定数Ｋｐ＿ｒｅｆに補正を加える仮想バネ定数制御手法を適用した補正処理を行う。具体的には、補正処理部１０は、位置目標値Ｐｏｓ＿ｒｅｆと仮想バネ定数Ｋｐ＿ｒｅｆの少なくとも一方についてモータに加わる外乱要因に対応した補正を行う。以下の説明では、モータに加わる外乱に起因して変動するパラメータのうちトルクを指示するパラメータを用いて補正処理の切り替えを行う例を説明するが、外乱に起因してモータに生じる変化を捉えられるパラメータのうち周波数が異なる成分を含むパラメータであれば補正処理の切り替えに用いることは可能である。

【0020】

さらに具体的には、実施の形態１にかかるロボット駆動装置１では、トルク変動の周波数特性を解析し、低周波側のトルク変動に対しては位置目標値の補正を行う位置制御手法を用いた第１の補正処理を行い、高周波側のトルク変動に対しては仮想バネ定数の補正を行う仮想バネ定数制御手法を用いた第２の補正処理を行う。そこで、位置制御手法と仮想バネ定数制御手法の制御ロジックについて説明する。

【0021】

図２に位置目標値を変化させる場合（位置制御手法）の制御ロジックを説明する図を示す。なお、図２では、仮想バネ定数は一定に維持されるものとする。図２に示す例では、位置現在値と、位置の目標値である位置目標値Ｐｏｓ＿ｒｅｆとの差が位置差Ｐ１に達した時点で推定トルクτｃｕｒがトルク目標値τｒｅｆに達する。そして、位置制御手法では、位置現在値と位置目標値Ｐｏｓ＿ｒｅｆとの差がさらに拡大した場合には、位置目標値Ｐｏｓ＿ｒｅｆを、位置現在値との差が一定になるように補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ（図２の補正あり目標位置）に補正する。位置制御手法を適用しない場合、現在位置値と位置目標値Ｐｏｓ＿ｒｅｆとの位置差が位置差Ｐ１より拡大すると位置現在値を位置目標値Ｐｏｓ＿ｒｅｆに近づけるために推定トルクがトルク目標値τｒｅｆよりも高くなる（図２の補正無しトルク）。一方、この位置制御手法を適用すると、位置現在値と位置目標値Ｐｏｓ＿ｒｅｆとの位置差が位置差Ｐ１より拡大しても補正後の推定トルクはトルク目標値τｒｅｆで維持される。

【0022】

続いて、図３に仮想バネ定数を変化させる場合（仮想バネ定数制御手法）の制御ロジックを説明する図を示す。なお、図３に示す例では、位置目標値は一定に維持されるものとする。図３に示す例においても、位置現在値と、位置の目標値である位置目標値Ｐｏｓ＿ｒｅｆとの差が位置差Ｐ１に達した時点で推定トルクτｃｕｒがトルク目標値τｒｅｆに達する。そして、仮想バネ定数手法では、位置現在値と位置目標値Ｐｏｓ＿ｒｅｆとの差がさらに拡大した場合には、仮想バネ定数Ｋｐを位置差の大きさに応じて仮想バネ定数Ｋｐ＿ｒｅｆよりも徐々に小さくなるように補正する。仮想バネ定数制御手法を適用しない場合、現在位置値と位置目標値Ｐｏｓ＿ｒｅｆとの位置差が位置差Ｐ１より拡大すると位置現在値を位置目標値Ｐｏｓ＿ｒｅｆに近づけるために推定トルクがトルク目標値τｒｅｆよりも高くなる（図２の補正無しトルク）。一方、この仮想バネ定数制御手法を適用すると、位置現在値と位置目標値Ｐｏｓ＿ｒｅｆとの位置差が位置差Ｐ１より拡大しても補正後の推定トルクはトルク目標値τｒｅｆで維持される。

【0023】

図２及び図３で説明したように、位置制御手法も仮想バネ定数制御手法もいずれの手法であっても、トルク目標値τｒｅｆを上限とするように推定トルクを制限することはできる。しかしながら、位置制御手法は、低い周波数で変動するトルク制御により効果的であり、仮想バネ定数制御手法は高い周波数で変動するトルク制御により効果的であるという違いがある。そこで、実施の形態１にかかるロボット駆動装置１では、補正処理部１０において、低周波側のトルク変動に対しては位置目標値の補正を行う位置制御手法を用いた第１の補正処理を行い、高周波側のトルク変動に対しては仮想バネ定数の補正を行う仮想バネ定数制御手法を用いた第２の補正処理を行う。そこで、補正処理部１０の構成及び動作について以下でさらに詳細に説明する。

【0024】

補正処理部１０は、補正処理を行うに当たり、参照処理、第１の補正処理及び第２の補正処理を行う。参照処理では、予め設定される値であって、モータに与えられる外乱により変動する外乱指標の目標値である外乱指標目標値を参照する。以下の説明では、外乱指標としてトルク値を用いる、そこで補正処理部１０では、外乱指標の目標値としてトルク目標値τｒｅｆを参照する。また、補正処理部１０では、モータのトルクとして、モータから得た位置現在値を用いた演算から得られる推定値を用いる。このトルクの推定値を以下の説明では推定トルクと称する。なお、以下で説明する補正処理部１０では、モータ駆動回路１２がモータ制御において取得する回転角に基づき出力する位置現在値Ｐｏｓ＿ｃｕｒを用いて推定トルクを算出する例を説明するが、トルクセンサや電流センサ等を用いて取得されるパラメータから得られる値を以下で説明する推定トルクとして利用することもできる。なお、トルクセンサは、応答周波数が低い（例えば、１ＫＨｚ以下）ため、トルクセンサから得られたトルク値は、以下で説明する低周波側推定トルクτｃｕｒ＿Ｌの代替値として好適である。また、電流センサは、応答周波数が高い（例えば、２００ＫＨｚ以下）ため、電流センサから得られた電流値から算出する推定トルクは、以下で説明する高周波側推定トルクτｃｕｒ＿Ｈの代替値として好適である。

【0025】

第１の補正処理では、外乱指標（推定トルク）の成分うち低周波側の周波数特性を有する低周波側外乱成分（例えば、低周波側推定トルクτｃｕｒ＿Ｌ）が外乱指標目標値（例えば、トルク目標値τｒｅｆ）以上であれば、予め設定された算出式に基づき算出される値を補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃに設定し、低周波側外乱成分が外乱指標目標値よりも小さければ予め設定される初期値（例えば、位置目標値Ｐｏｓ＿ｒｅｆ）を補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃに設定する。

【0026】

第２の補正処理では、外乱指標（推定トルク）の成分うち高周波側の周波数特性を有する高周波側外乱成分（例えば、高周波側推定トルクτｃｕｒ＿Ｈ）が外乱指標目標値（例えば、トルク目標値τｒｅｆ）以上であれば、予め設定された算出式に基づき算出される値を補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃに設定し、低周波側外乱成分が外乱指標目標値よりも小さければ予め設定される初期値を補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃに設定する。

【0027】

上記動作を実現する補正処理部１０の構成例について説明する。そこで、図４に実施の形態１にかかる補正処理部１０のブロック図を示す。図４に示すように、補正処理部１０は、補正位置算出部３１、補正Ｋｐ算出部３２、ディレイ回路３３、３４、フィルタ処理部３５を有する。なお、実施の形態１にかかるロボット駆動装置１では、制御指令値生成部１１が所定の周期でモータ駆動回路１２に与える制御指令値を生成するため、補正処理部１０も制御指令値生成部１１の周期に同期した周期で補正処理を行う。図４及び以下の説明では、ｎを現周期、ｎ－１を前周期の値であることを示す符号として示した。

【0028】

補正位置算出部３１は、参照処理と第１の補正処理とを行う。具体的には、参照処理では、補正位置算出部３１は、与えられるトルク目標値τｒｅｆを参照する。また、補正位置算出部３１は、トルク目標値τｒｅｆと、フィルタ処理部３５において算出された低周波側推定トルクτｃｕｒ＿Ｌと、を比較し、低周波側推定トルクτｃｕｒ＿Ｌがトルク目標値τｒｅｆ以上である場合、（１）式に基づき位置目標値Ｐｏｓ＿ｒｅｆの補正値である補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃを算出する。
Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］＝（τｃｕｒ＿Ｌ－τｒｅｆ）＊Ｋｐ＿ｒｅｆ＊Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］・・・（１）
なお、（１）式においてＫｐ＿ｒｅｆは、仮想バネ定数Ｋｐ＿ｒｅｆである。一方、補正位置算出部３１は、低周波側推定トルクτｃｕｒ＿Ｌがトルク目標値τｒｅｆよりも小さい場合、（２）式に基づき位置目標値Ｐｏｓ＿ｒｅｆの補正値である補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃを算出する。
Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］＝Ｐｏｓ＿ｒｅｆ・・・（２）
つまり、補正位置算出部３１は、低周波側推定トルクτｃｕｒ＿Ｌがトルク目標値τｒｅｆよりも小さい場合、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃを位置目標値Ｐｏｓ＿ｒｅｆに戻す、或いは、維持する。

【0029】

補正Ｋｐ算出部３２は、参照処理と第２の補正処理とを行う。具体的には、参照処理では、補正Ｋｐ算出部３２は、与えられるトルク目標値τｒｅｆを参照する。また、補正Ｋｐ算出部３２は、トルク目標値τｒｅｆと、フィルタ処理部３５において算出された高周波側推定トルクτｃｕｒ＿Ｈと、を比較し、高周波側推定トルクτｃｕｒ＿Ｈがトルク目標値τｒｅｆ以上である場合、（３）式に基づき仮想バネ定数Ｋｐ＿ｒｅｆの補正値である補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを算出する。
Ｋｐ＿ｒｅｆ［ｎ］＝τｒｅｆ／（Ｐｏｓ＿σ＿Ｈ）・・・（３）
なお、（３）式のＰｏｓ＿σ＿Ｈは、高周波側の位置偏差であり、（４）式で表わされる。
Ｐｏｓ＿σ＿Ｈ＝Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］－Ｐｏｓ＿ｃｕｒ＿Ｌ・・・（４）
（４）式においてＰｏｓ＿ｃｕｒ＿Ｌは、高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈである。一方、補正Ｋｐ算出部３２は、高周波側推定トルクτｃｕｒ＿Ｈがトルク目標値τｒｅｆよりも小さい場合、（５）式に基づき仮想バネ定数Ｋｐ＿ｒｅｆの補正値である補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを算出する。
Ｋｐ＿ｒｅｆ＿ｃ［ｎ］＝Ｋｐ＿ｒｅｆ・・・（５）
つまり、補正Ｋｐ算出部３２は、高周波側推定トルクτｃｕｒ＿Ｈがトルク目標値τｒｅｆよりも小さい場合、補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを仮想バネ定数Ｋｐ＿ｒｅｆに戻す、或いは、維持する。

【0030】

ディレイ回路３３は、補正位置算出部３１が出力した補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃを次の処理周期まで保持する。ディレイ回路３４は、補正Ｋｐ算出部３２が出力した補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃを次の処理周期まで保持する。

【0031】

フィルタ処理部３５は、関節の位置偏差又はモータのトルク偏差を高周波側と低周波側の成分に分離する。具体的には、フィルタ処理部３５は、モータから位置現在値Ｐｏｓ＿ｃｕｒを取得し、取得した位置現在値Ｐｏｓ＿ｃｕｒに対してフィルタ処理を施して、低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌと高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈとを算出する。そして、フィルタ処理部３５は、低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌを用いて低周波側位置偏差と低周波側推定トルクτｃｕｒ＿Ｌを算出する。また、フィルタ処理部３５は、高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを用いて高周波側位置偏差Ｐｏｓ＿σ＿Ｈと高周波側推定トルクτｃｕｒ＿Ｈを算出する。

【0032】

上記演算を行うために、フィルタ処理部３５は、ローパスフィルタ４１、減算器４２。乗算器４３、ハイパフィルタ４４、減算器４５、乗算器４６を有する。ローパスフィルタ４１は、位置現在値Ｐｏｓ＿ｃｕｒに対してローパスフィルタ処理を施して、低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌを出力する。減算器４２は、補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］から低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌを減算して低周波側位置偏差を出力する。乗算器４３は、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］に減算器４２が出力した低周波側位置偏差を乗算して低周波側推定トルクτｃｕｒ＿Ｌを出力する。

【0033】

ハイパフィルタ４４は、位置現在値Ｐｏｓ＿ｃｕｒに対してハイパス処理を施して、高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを出力する。減算器４５は、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］から高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを減算して高周波側位置偏差Ｐｏｓ＿σ＿Ｈを出力する。乗算器４６は、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］に減算器４５が出力した高周波側位置偏差Ｐｏｓ＿σ＿Ｈを乗算して高周波側推定トルクτｃｕｒ＿Ｈを出力する。

【0034】

上記処理を行う順序について具体的に説明を行う。そこで、図５に実施の形態１にかかる補正処理部１０の動作を説明するフローチャートを示す。なお、補正処理部１０は、図５で示した処理を制御指令値生成部１１の処理周期に同期した周期で繰り返す。

【0035】

図５に示すように、補正処理部１０は、処理を開始すると、まず、位置現在値Ｐｏｓ＿ｃｕｒを取得する（ステップＳ１）。続いて、補正処理部１０は、ローパスフィルタ４１において、位置現在値Ｐｏｓ＿ｃｕｒから低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌを算出する（ステップＳ２）。また、補正処理部１０は、補正処理部１０は、ハイパフィルタ４４において、位置現在値Ｐｏｓ＿ｃｕｒから高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを算出する（ステップＳ３）。

【0036】

次いで、補正処理部１０は、フィルタ処理部３５において、補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］と、低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌと、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］と、を用いて低周波側推定トルクτｃｕｒ＿Ｌを算出する（ステップＳ４）。その後、補正処理部１０は、補正位置算出部３１において、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］を算出する（ステップＳ５）。このステップＳ５では、補正位置算出部３１は、低周波側推定トルクτｃｕｒ＿Ｌとトルク目標値τｒｅｆとの大小関係に基づき（１）式と（２）式の何れか一方の計算式を選択して補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］を算出する。

【0037】

次いで、補正処理部１０は、フィルタ処理部３５において、ステップＳ５で算出された補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］と、高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈと、を用いて高周波側位置偏差Ｐｏｓ＿σ＿Ｈを算出する（ステップＳ６）。また、補正処理部１０は、フィルタ処理部３５において、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］と、高周波側位置偏差Ｐｏｓ＿σ＿Ｈと、を用いて高周波側推定トルクτｃｕｒ＿Ｈを算出する（ステップＳ７）。その後、補正処理部１０は、補正Ｋｐ算出部３２において、補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］を算出する（ステップＳ８）。このステップＳ８では、補正Ｋｐ算出部３２は、高周波側推定トルクτｃｕｒ＿Ｈとトルク目標値τｒｅｆとの大小関係に基づき（３）式と（５）式の何れか一方の計算式を選択して補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］を算出する。

【0038】

上記説明より、実施の形態１にかかるロボット駆動装置１では、補正処理部１０が外乱により生じるトルク変動を低周波成分と高周波成分とに分離し、低周波成分に対しては位置制御手法を適用し、高周波成分に対しては仮想バネ定数制御手法を適用した制御を行う。これにより、実施の形態１にかかるロボット駆動装置１では、位置制御手法と仮想バネ定数制御手法との何れか一方のみを適用した制御手法における制限がなくすことが可能になる。そして、実施の形態１にかかるロボット駆動装置１に位置制御手法と仮想バネ定数制御手法との何れか一方のみを適用した制御手法に比べて高い安定性を実現することができる。

【0039】

具体的には、実施の形態１にかかるロボット駆動装置１は、モータにより駆動されるアーム等が物体に衝突或いは接触した時に安定的に位置とトルクをコントロールすることができる。また、実施の形態１にかかるロボット駆動装置１は、モータにより駆動されるアーム等が物体に衝突或いは接触した後にさらに位置偏差が大きくなっていく場合にトルクを一定にすることができる。このような制御により、実施の形態１にかかるロボット駆動装置１では、アームが物体に衝突或いは接触した際に物体を破損させるような事故を防ぐことが可能になる。

【0040】

実施の形態２
実施の形態２では、実施の形態１の補正処理部１０の別の形態となる補正処理部１０ａについて説明する。なお、実施の形態２の説明において実施の形態１の説明で説明した構成要素については実施の形態１と同じ符号を付して説明を省略する。

【0041】

図６に実施の形態２にかかる補正処理部１０ａのブロック図を示す。図６に示すように、実施の形態２にかかる補正処理部１０ａは、補正位置算出部３１、補正Ｋｐ算出部３２及びフィルタ処理部３５を補正パラメータ算出部５１及びフィルタ処理部５５に置き換えたものである。補正パラメータ算出部５１は、補正位置算出部３１で行っていた第１の補正処理と補正Ｋｐ算出部３２で行っていた第２の補正処理とを行う。

【0042】

ここで、補正パラメータ算出部５１では、低周波側推定トルクτｃｕｒ＿Ｌが高周波側推定トルクτｃｕｒ＿Ｈ以上であれば（１）式及び（２）式に基づき補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］を算出する第１の補正処理を行う。また、補正パラメータ算出部５１は、第１の補正処理とともに、予め設定される初期値（例えば、仮想バネ定数Ｋｐ＿ｒｅｆ）を補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］に設定する。

【0043】

また、補正パラメータ算出部５１では、低周波側推定トルクτｃｕｒ＿Ｌが高周波側推定トルクτｃｕｒ＿Ｈよりも小さければ（３）式及び（５）式に基づき補正後位仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］を算出する第２の補正処理を行う。また、補正パラメータ算出部５１は、第２の補正処理とともに、予め設定される初期値（例えば、位置目標値Ｐｏｓ＿ｒｅｆ）を補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］に設定する。

【0044】

フィルタ処理部５５は、減算器４５を減算器６５に置き換えたものである。フィルタ処理部５５では、減算器４５において、補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］から高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを減算することで、高周波側位置偏差Ｐｏｓ＿σ＿Ｈを算出する。これは、実施の形態２では、仮想バネ定数Ｋｐ＿ｒｅｆの補正値の算出に前周期で算出された補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］を利用することで、補正パラメータ算出部５１が高周波側推定トルクτｃｕｒ＿Ｈと低周波側推定トルクτｃｕｒ＿Ｌとの大小関係に基づき位置目標値Ｐｏｓ＿ｒｅｆと仮想バネ定数Ｋｐ＿ｒｅｆの何れかに対して補正処理を行うことができる。

【0045】

このように、実施の形態２にかかる補正処理部１０ａの動作は、補正値を算出するための処理フローが異なる。そこで、図７に実施の形態２にかかる補正処理部１０ａの動作を説明するフローチャートし、補正処理部１０ａの動作について説明する。

【0046】

図７に示すように、補正処理部１０ａは、処理を開始すると、まず、位置現在値Ｐｏｓ＿ｃｕｒを取得する（ステップＳ１１）。続いて、補正処理部１０ａは、ローパスフィルタ４１において、位置現在値Ｐｏｓ＿ｃｕｒから低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌを算出する（ステップＳ１２）。また、補正処理部１０ａは、補正処理部１０は、ハイパフィルタ４４において、位置現在値Ｐｏｓ＿ｃｕｒから高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈを算出する（ステップＳ１３）。

【0047】

次いで、補正処理部１０ａは、フィルタ処理部５５において、補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］と、低周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｌと、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］と、を用いて低周波側推定トルクτｃｕｒ＿Ｌを算出する（ステップＳ１４）。また、補正処理部１０ａは、フィルタ処理部５５において、補正後位置目標前回値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ－１］と、高周波側位置現在値Ｐｏｓ＿ｃｕｒ＿Ｈと、を用いて高周波側位置偏差Ｐｏｓ＿σ＿Ｈを算出する（ステップＳ１５）。そして、補正処理部１０ａは、フィルタ処理部５５において、補正後仮想バネ定数前回値Ｋｐ＿ｒｅｆ＿ｃ［ｎ－１］と、高周波側位置偏差Ｐｏｓ＿σ＿Ｈと、を用いて高周波側推定トルクτｃｕｒ＿Ｈを算出する（ステップＳ１６）。

【0048】

その後、補正処理部１０ａは、補正パラメータ算出部５１において、低周波側推定トルクτｃｕｒ＿Ｌと高周波側推定トルクτｃｕｒ＿Ｈとの大小比較を行う（ステップＳ１７）。そして、低周波側推定トルクτｃｕｒ＿Ｌが高周波側推定トルクτｃｕｒ＿Ｈ以上の大きさであれば、第１の補正処理により、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］を算出する（ステップＳ１８）。また、図示はしていないが、ステップＳ１８では、補正処理部１０ａは、第１の補足補正処理により、補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］として仮想バネ定数Ｋｐ＿ｒｅｆを出力する。一方、低周波側推定トルクτｃｕｒ＿Ｌが高周波側推定トルクτｃｕｒ＿Ｈよりも小さければ、第２の補正処理により、補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃ［ｎ］を算出する（ステップＳ１９）。また、図示はしていないが、ステップＳ１９では、補正処理部１０ａは、第２の補足補正処理により、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ［ｎ］として位置目標値Ｐｏｓ＿ｒｅｆを出力する。

【0049】

上記説明より、実施の形態２にかかる補正処理部１０ａを含むロボット駆動装置は、補正後位置目標値Ｐｏｓ＿ｒｅｆ＿ｃ及び補正後仮想バネ定数Ｋｐ＿ｒｅｆ＿ｃの算出に要する計算量が実施の形態１にかかる補正処理部１０よりも少ない。これにより、実施の形態２にかかる補正処理部１０ａは、実施の形態１の補正処理部１０よりも演算負荷を低減することができる。また、実施の形態２にかかる補正処理部１０ａは、実施の形態１の補正処理部１０よりも演算負荷が少ないため、より高速な周期で動作を行う事が可能になる。

【0050】

以上、本発明者によってなされた発明を実施の形態に基づき具体的に説明したが、本発明は既に述べた実施の形態に限定されるものではなく、その要旨を逸脱しない範囲において種々の変更が可能であることはいうまでもない。

【符号の説明】

【0051】

１ ロボット駆動装置
１０、１０ａ 補正処理部
１１ 制御指令値生成部
１２ モータ駆動回路
２１、２４、４２、４５、６５ 減算器
２２、２５、４３、４６ 乗算器
２３ 現在速度算出部
２６、２７ 加算器
３１ 補正位置算出部
３２ 補正Ｋｐ算出部
３３、３４ ディレイ回路
３４ ディレイ回路
３５ フィルタ処理部
４１ ローパスフィルタ
４４ ハイパフィルタ
５１ 補正パラメータ算出部
５５ フィルタ処理部
Ｐｏｓ＿ｃｕｒ 位置現在値
Ｐｏｓ＿ｒｅｆ 位置目標値
Ｐｏｓ＿ｒｅｆ＿ｃ 補正後位置目標値
Ｋｐ＿ｒｅｆ 仮想バネ定数
Ｋｐ＿ｒｅｆ＿ｃ 補正後仮想バネ定数
Ｖｅｌ＿ｒｅｆ 速度目標値
Ｋｄ＿ｒｅｆ 制御ゲイン
τｒｅｆ トルク目標値
τｃｕｒ＿Ｌ 低周波側推定トルク
τｃｕｒ＿Ｈ 高周波側推定トルク

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】