JPH10217173A - ロボットの非干渉化制御装置 - Google Patents
ロボットの非干渉化制御装置Info
- Publication number
- JPH10217173A JPH10217173A JP2551497A JP2551497A JPH10217173A JP H10217173 A JPH10217173 A JP H10217173A JP 2551497 A JP2551497 A JP 2551497A JP 2551497 A JP2551497 A JP 2551497A JP H10217173 A JPH10217173 A JP H10217173A
- Authority
- JP
- Japan
- Prior art keywords
- decoupling
- arm
- robot
- axis
- control device
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
(57)【要約】
【課題】 安定した軌跡精度を得ることのできる多軸ロ
ボットの非干渉化制御装置を提供する。 【解決手段】 少なくとも２本以上のアームが関節部を
介し、シリアルに結合された構造を持つロボットを制御
する多軸ロボット制御装置において、各アームの角度
と、各アームの質量、長さ等の動力学パラメータとから
慣性行列を作成する慣性行列計算機構３１と、各アーム
の加速度を含む非干渉化用状態変数を演算する演算機構
３３と、これら状態変数に乗算する非干渉化ゲインを演
算する演算機構３２と、前記非干渉化用状態変数と前記
非干渉化ゲインとを乗算及び加算する乗算部３４及び加
算部３５と、その計算の結果得られる非干渉化トルク指
令から高調波リップル分を除去するためのフィルタ処理
部３６を有するロボットの非干渉化制御装置。
ボットの非干渉化制御装置を提供する。 【解決手段】 少なくとも２本以上のアームが関節部を
介し、シリアルに結合された構造を持つロボットを制御
する多軸ロボット制御装置において、各アームの角度
と、各アームの質量、長さ等の動力学パラメータとから
慣性行列を作成する慣性行列計算機構３１と、各アーム
の加速度を含む非干渉化用状態変数を演算する演算機構
３３と、これら状態変数に乗算する非干渉化ゲインを演
算する演算機構３２と、前記非干渉化用状態変数と前記
非干渉化ゲインとを乗算及び加算する乗算部３４及び加
算部３５と、その計算の結果得られる非干渉化トルク指
令から高調波リップル分を除去するためのフィルタ処理
部３６を有するロボットの非干渉化制御装置。
Description
【０００１】
【産業上の利用分野】本発明は、少なくとも２本以上の
アームが関節部を介し、シリアルに結合された構造を持
つロボットにおいて、軸間干渉トルクによる軌跡精度の
悪化を効果的に低減することのできるロボットの非干渉
化制御装置に関するものである。
アームが関節部を介し、シリアルに結合された構造を持
つロボットにおいて、軸間干渉トルクによる軌跡精度の
悪化を効果的に低減することのできるロボットの非干渉
化制御装置に関するものである。
【０００２】
【従来の技術】一般的な産業用ロボットは、図３に示す
ように、アクチュエータに電動機１０１を用い、その回
転軸に減速機構１０２を介してアーム１０３を接続した
構造を持っている。減速機構１０２は、アーム１０３の
イナーシャと軸間干渉トルクを含む外乱の影響を小さく
できるよう大きな減速比と減速機構ゆえのバネ特性を持
っている。従来においては、ロボットの動作指令の生成
時に、加減速度を小さくするあるいは速度制限を低くす
ることで、軸間干渉トルクの影響を小さくするような消
極的な方法が取られていたが、動作時間をできるだけ短
縮したいとの要求から、指令加減速度が大きくなると共
に減速機構のバネ特性によりアーム側の速度、加速度が
さらに大きくなるため、軸間干渉トルクが大きくなる。
そのため、減速機構のバネ特性によってアームのねじれ
が大きくなること、また減速比が大きいにも拘わらず電
動機側のフィードバック制御部に大きな外乱として印加
されることで、軌跡精度が悪化する。このような問題に
対する従来例としては、他軸のトルク指令や速度推定値
から自軸への干渉トルク相当分を計算し、自軸のトルク
指令に加算する非干渉化方式が特開昭６３−３１４６０
６号公報に開示されている。
ように、アクチュエータに電動機１０１を用い、その回
転軸に減速機構１０２を介してアーム１０３を接続した
構造を持っている。減速機構１０２は、アーム１０３の
イナーシャと軸間干渉トルクを含む外乱の影響を小さく
できるよう大きな減速比と減速機構ゆえのバネ特性を持
っている。従来においては、ロボットの動作指令の生成
時に、加減速度を小さくするあるいは速度制限を低くす
ることで、軸間干渉トルクの影響を小さくするような消
極的な方法が取られていたが、動作時間をできるだけ短
縮したいとの要求から、指令加減速度が大きくなると共
に減速機構のバネ特性によりアーム側の速度、加速度が
さらに大きくなるため、軸間干渉トルクが大きくなる。
そのため、減速機構のバネ特性によってアームのねじれ
が大きくなること、また減速比が大きいにも拘わらず電
動機側のフィードバック制御部に大きな外乱として印加
されることで、軌跡精度が悪化する。このような問題に
対する従来例としては、他軸のトルク指令や速度推定値
から自軸への干渉トルク相当分を計算し、自軸のトルク
指令に加算する非干渉化方式が特開昭６３−３１４６０
６号公報に開示されている。
【０００３】
【発明が解決しようとする課題】軸間干渉トルクが、電
動機への外乱トルクとなることを図４を用いて説明す
る。図４は図３に示したロボットアームの構造と制御構
造をブロック図で示したものである。軸間干渉トルク５
４は、アーム１０３に印加され、それがアーム角速度５
５となり、電動機１０１の角速度５１との間にねじれ角
速度５２を発生させる。減速機１０２へのねじれは、ア
ーム１０３への駆動トルク５２となり、それが図４に示
す通り、同時に電動機１０１への外乱トルク５０とな
る。これは、軸間干渉トルクのみならず、重力、コリオ
リ力についても同様である。つまり、アームへの軸間干
渉トルクを代表とする外乱は、産業用ロボットの持つ減
速機構のバネ特性によって、いったん電動機とアーム間
のねじれトルクに変えられて、電動機に伝わることにな
る。このようなアームへの軸間干渉トルクを、図４に示
すような電動機への非干渉化トルク補償値４５によっ
て、アーム側の位置、速度への影響を打ち消すために
は、非干渉化トルク補償値が、いったん電動機とアーム
間のねじれトルクとなり、アームへの軸間干渉トルクと
相殺される必要がある。従って、従来例の様に、減速機
構のバネ特性を考慮しない方法では、効果的にアーム側
の位置、速度への影響を打ち消すことができないことが
わかる。
動機への外乱トルクとなることを図４を用いて説明す
る。図４は図３に示したロボットアームの構造と制御構
造をブロック図で示したものである。軸間干渉トルク５
４は、アーム１０３に印加され、それがアーム角速度５
５となり、電動機１０１の角速度５１との間にねじれ角
速度５２を発生させる。減速機１０２へのねじれは、ア
ーム１０３への駆動トルク５２となり、それが図４に示
す通り、同時に電動機１０１への外乱トルク５０とな
る。これは、軸間干渉トルクのみならず、重力、コリオ
リ力についても同様である。つまり、アームへの軸間干
渉トルクを代表とする外乱は、産業用ロボットの持つ減
速機構のバネ特性によって、いったん電動機とアーム間
のねじれトルクに変えられて、電動機に伝わることにな
る。このようなアームへの軸間干渉トルクを、図４に示
すような電動機への非干渉化トルク補償値４５によっ
て、アーム側の位置、速度への影響を打ち消すために
は、非干渉化トルク補償値が、いったん電動機とアーム
間のねじれトルクとなり、アームへの軸間干渉トルクと
相殺される必要がある。従って、従来例の様に、減速機
構のバネ特性を考慮しない方法では、効果的にアーム側
の位置、速度への影響を打ち消すことができないことが
わかる。
【０００４】また、産業用ロボットの全体的な構造は、
図５の様に少なくとも２本以上のアームが関節部を介
し、シリアルに結合された構造を持っており、軸間干渉
の該当軸同士以外の軸の関節角の値によって、軸間干渉
トルクの値が変化する。一方、減速機構のバネ特性を考
慮する場合には、アームに印加される軸間干渉トルク
と、その積分値、微分値、２回微分値が必要になる。こ
れらについての詳細は実施例で述べる。補償に必要な軸
間干渉トルク値は、従来例のような干渉トルクを発生さ
せる軸のトルク指令から求める方法や、状態推定オブザ
ーバを使用して求める方法が一般的であるため、軸間干
渉トルクの微分値や２回微分値を求めるのに差分を用い
ると、電動機の位置や速度検出器の分解能や軸間干渉の
補償計算時間によるサンプリング時間の影響で、リップ
ルが大きく、軸間干渉トルク補償に使えないものとなっ
てしまう。本発明では、このような従来例の問題点を払
拭したロボットの非干渉化制御装置を提供することを目
的とする。
図５の様に少なくとも２本以上のアームが関節部を介
し、シリアルに結合された構造を持っており、軸間干渉
の該当軸同士以外の軸の関節角の値によって、軸間干渉
トルクの値が変化する。一方、減速機構のバネ特性を考
慮する場合には、アームに印加される軸間干渉トルク
と、その積分値、微分値、２回微分値が必要になる。こ
れらについての詳細は実施例で述べる。補償に必要な軸
間干渉トルク値は、従来例のような干渉トルクを発生さ
せる軸のトルク指令から求める方法や、状態推定オブザ
ーバを使用して求める方法が一般的であるため、軸間干
渉トルクの微分値や２回微分値を求めるのに差分を用い
ると、電動機の位置や速度検出器の分解能や軸間干渉の
補償計算時間によるサンプリング時間の影響で、リップ
ルが大きく、軸間干渉トルク補償に使えないものとなっ
てしまう。本発明では、このような従来例の問題点を払
拭したロボットの非干渉化制御装置を提供することを目
的とする。
【０００５】
【課題を解決するための手段】上記課題を解決するため
に、本発明では、少なくとも２本以上のアームが関節部
を介し、シリアルに結合された構造を持つロボットを制
御する多軸ロボット制御装置において、各アームの角度
と、各アームの質量、長さ等の動力学パラメータとから
慣性行列を作成する慣性行列計算機構と、各アームの加
速度を含む非干渉化用状態変数を演算する演算機構と、
これら状態変数に乗算する非干渉化ゲインを演算する演
算機構と、前記非干渉化用状態変数と前記非干渉化ゲイ
ンとを乗算及び加算する乗算部及び加算部と、その計算
の結果得られる非干渉化トルク指令から高調波リップル
分を除去するためのフィルタ処理部を有するようにして
いる。望ましくは、前記各アームの加速度を含む非干渉
化用状態変数の演算機構は、各軸アームへの関節角指令
値を用い、前記非干渉化トルク指令のフィルタ処理部
は、２次フィルタを用いることで、電動機の位置や速度
検出器の分解能に影響されず計算量が少なくなる。さら
に、前記各アームの加速度を含む非干渉化用状態変数の
演算機構は、状態推定オブザーバを使用することで、ア
ーム加速度の積分手段、１回微分手段、２回微分手段を
１回微分手段のみにし、リップルと計算量を低減できる
ようにする。本発明は、上記した軸間干渉トルクの補償
手段により、ロボットの軸間の干渉状態を監視し、ロボ
ット減速機構のバネ特性を考慮し、軸間干渉トルクの補
償トルク信号のリップルを低減することで、安定した軌
跡精度を得ることのできるロボットの非干渉化制御装置
を得ることができる。
に、本発明では、少なくとも２本以上のアームが関節部
を介し、シリアルに結合された構造を持つロボットを制
御する多軸ロボット制御装置において、各アームの角度
と、各アームの質量、長さ等の動力学パラメータとから
慣性行列を作成する慣性行列計算機構と、各アームの加
速度を含む非干渉化用状態変数を演算する演算機構と、
これら状態変数に乗算する非干渉化ゲインを演算する演
算機構と、前記非干渉化用状態変数と前記非干渉化ゲイ
ンとを乗算及び加算する乗算部及び加算部と、その計算
の結果得られる非干渉化トルク指令から高調波リップル
分を除去するためのフィルタ処理部を有するようにして
いる。望ましくは、前記各アームの加速度を含む非干渉
化用状態変数の演算機構は、各軸アームへの関節角指令
値を用い、前記非干渉化トルク指令のフィルタ処理部
は、２次フィルタを用いることで、電動機の位置や速度
検出器の分解能に影響されず計算量が少なくなる。さら
に、前記各アームの加速度を含む非干渉化用状態変数の
演算機構は、状態推定オブザーバを使用することで、ア
ーム加速度の積分手段、１回微分手段、２回微分手段を
１回微分手段のみにし、リップルと計算量を低減できる
ようにする。本発明は、上記した軸間干渉トルクの補償
手段により、ロボットの軸間の干渉状態を監視し、ロボ
ット減速機構のバネ特性を考慮し、軸間干渉トルクの補
償トルク信号のリップルを低減することで、安定した軌
跡精度を得ることのできるロボットの非干渉化制御装置
を得ることができる。
【０００６】
【実施例】以下、本発明の一実施例を図面に基づいて説
明する。図１は、本発明の請求項３の非干渉化制御装置
をブロック図で示したものである。なお、図中及び文中
で使用する記号は、次の通りとする。 θr1，θr2，・・・，θrn：各軸の関節角指令値 Ｉ1，Ｉ2，・・・，Ｉn：各軸の電流指令 Ｋijk：非干渉化ゲイン行列要素 Ｘik：非干渉化用状態変数 Ｔcri：非干渉化トルク指令 Ｔci：非干渉化トルク補正値 Ｊm：電動機ロータイナーシヤ Ｋ：減速機構バネ定数 ＪL：アーム側イナーシャ Ｍij：干渉イナーシャ θm：電動機角度 θL：アーム角度 θS：電動機とアームのねじれ角（＝θm−θL） Ｖref：速度指令 Ｔref：トルク指令 Ｔd：軸間干渉トルク Ｔg：重カモーメント θmr：電動機への角度指令 Ｋp：位置ループ比例ゲイン Ｋv：速度ループ比例ゲイン Ｋi：速度ループ積分ゲイン ωm：電動機角速度 ωL：アーム角速度 添字ｉ，ｊ：軸間干渉の該当軸 添字ｋ：非干渉化用の該当状態変数 添字ｎ：ロボツト軸
明する。図１は、本発明の請求項３の非干渉化制御装置
をブロック図で示したものである。なお、図中及び文中
で使用する記号は、次の通りとする。 θr1，θr2，・・・，θrn：各軸の関節角指令値 Ｉ1，Ｉ2，・・・，Ｉn：各軸の電流指令 Ｋijk：非干渉化ゲイン行列要素 Ｘik：非干渉化用状態変数 Ｔcri：非干渉化トルク指令 Ｔci：非干渉化トルク補正値 Ｊm：電動機ロータイナーシヤ Ｋ：減速機構バネ定数 ＪL：アーム側イナーシャ Ｍij：干渉イナーシャ θm：電動機角度 θL：アーム角度 θS：電動機とアームのねじれ角（＝θm−θL） Ｖref：速度指令 Ｔref：トルク指令 Ｔd：軸間干渉トルク Ｔg：重カモーメント θmr：電動機への角度指令 Ｋp：位置ループ比例ゲイン Ｋv：速度ループ比例ゲイン Ｋi：速度ループ積分ゲイン ωm：電動機角速度 ωL：アーム角速度 添字ｉ，ｊ：軸間干渉の該当軸 添字ｋ：非干渉化用の該当状態変数 添字ｎ：ロボツト軸
【０００７】この図１に示すロボットの非干渉化制御装
置での実施例では、コントローラ上位側２ａでロボット
１の手先目標値２１から、逆変換により、各軸の関節角
指令値２２を求め、これをコントローラ下位側２ｂに転
送し、各軸の位置、速度のフィードバック制御を行うデ
ィジタル制御装置２に、非干渉化制御装置３を接続して
構成されている。すなわち、各軸の関節角指令値２２か
ら求められる各軸のアームの角度とアームの動力学パラ
メータを格納した動力学データ２３から慣性行列計算機
構３１で慣性行列を作成し、その慣性行列要素Ｍij４１
と制御ゲインデータ２４を基に非干渉化ゲイン演算機構
３２で各関節毎のゲイン行列Ｋijkを求めておく。ここ
で添字ｉ，ｊはロボットｉｊ軸間を、ｋは非干渉化用状
態変数を示す。次に、前記関節角指令値２２を基に非干
渉化用状態変数演算機構３３で後述する非干渉化用状態
変数Ｘikを求め、それと前記ゲインＫijkを乗算器３４
で乗算し、その結果を各軸毎に加算器３５で加算し、各
軸毎の非干渉化トルク指令Ｔcr iを求める。計算された
非干渉化トルク指令Ｔcriは２次フィルタ３６で処理さ
れ、その結果得られた非干渉化トルク補正値Ｔciをディ
ジタル制御装置２の下位側２ｂのトルク指令に加算する
ことでロボットの非干渉化制御を実現している。
置での実施例では、コントローラ上位側２ａでロボット
１の手先目標値２１から、逆変換により、各軸の関節角
指令値２２を求め、これをコントローラ下位側２ｂに転
送し、各軸の位置、速度のフィードバック制御を行うデ
ィジタル制御装置２に、非干渉化制御装置３を接続して
構成されている。すなわち、各軸の関節角指令値２２か
ら求められる各軸のアームの角度とアームの動力学パラ
メータを格納した動力学データ２３から慣性行列計算機
構３１で慣性行列を作成し、その慣性行列要素Ｍij４１
と制御ゲインデータ２４を基に非干渉化ゲイン演算機構
３２で各関節毎のゲイン行列Ｋijkを求めておく。ここ
で添字ｉ，ｊはロボットｉｊ軸間を、ｋは非干渉化用状
態変数を示す。次に、前記関節角指令値２２を基に非干
渉化用状態変数演算機構３３で後述する非干渉化用状態
変数Ｘikを求め、それと前記ゲインＫijkを乗算器３４
で乗算し、その結果を各軸毎に加算器３５で加算し、各
軸毎の非干渉化トルク指令Ｔcr iを求める。計算された
非干渉化トルク指令Ｔcriは２次フィルタ３６で処理さ
れ、その結果得られた非干渉化トルク補正値Ｔciをディ
ジタル制御装置２の下位側２ｂのトルク指令に加算する
ことでロボットの非干渉化制御を実現している。
【０００８】具体的に説明する。説明を簡単にするため
に、図６に示す３軸ロボットを例にする。図６の様に、
第１関節から第３関節までの角度をそれぞれθ1、θ2、
θ3とし、手先負荷を質点ｍ3で代表させ、アームの長さ
や質点の距離を図のようにとる。一般的なロボットの運
動方程式は、Ｔをトルクとして、式（１）のように書け
る。
に、図６に示す３軸ロボットを例にする。図６の様に、
第１関節から第３関節までの角度をそれぞれθ1、θ2、
θ3とし、手先負荷を質点ｍ3で代表させ、アームの長さ
や質点の距離を図のようにとる。一般的なロボットの運
動方程式は、Ｔをトルクとして、式（１）のように書け
る。
【数１】 ここで、摩擦による項Ｃや重力による項ｇは除き、遠心
力、コリオリ力による項ｈは、慣性による項Ｊに比べて
小さいとし、ここでは軸間干渉の対象外とする。する
と、式（１）は慣性による項のみとなり、次の式（２）
のようになる。
力、コリオリ力による項ｈは、慣性による項Ｊに比べて
小さいとし、ここでは軸間干渉の対象外とする。する
と、式（１）は慣性による項のみとなり、次の式（２）
のようになる。
【数２】 図６の３軸ロボットの場合の慣性行列は式（３）のよう
になり、対角項以外のＭ23，Ｍ13，Ｍ12を、ここでは干
渉イナーシャと呼び、具体的に示す。ただし、角軸の重
心回りのイナーシャを無視した。
になり、対角項以外のＭ23，Ｍ13，Ｍ12を、ここでは干
渉イナーシャと呼び、具体的に示す。ただし、角軸の重
心回りのイナーシャを無視した。
【数３】 慣性行列計算機構３１は、関節角指令値２２から求めら
れる図６中のθ1、θ2、θ3のような角軸の角度と、
ｍ3，Ｌ1，Ｌ2，Ｌ3のような動力学パラメータを格納し
た動力学データ２３とを用いて、式（３）に示した慣性
行列の各要素、Ｍ11，Ｍ12，Ｍ13，Ｍ22，Ｍ23，Ｍ33を
計算する。
れる図６中のθ1、θ2、θ3のような角軸の角度と、
ｍ3，Ｌ1，Ｌ2，Ｌ3のような動力学パラメータを格納し
た動力学データ２３とを用いて、式（３）に示した慣性
行列の各要素、Ｍ11，Ｍ12，Ｍ13，Ｍ22，Ｍ23，Ｍ33を
計算する。
【０００９】次に、各アームの加速度を含む非干渉化用
状態変数の演算機構３３であるが、減速機構のバネ特性
や電動機を含むフィードバックループを考慮すると、軸
間干渉トルクを含めて４状態変数が必要になる。まずこ
のことを説明する。図４で示す位置・速度ループ２６を
除去して、運動方程式を求めると次の式（４）となる。
状態変数の演算機構３３であるが、減速機構のバネ特性
や電動機を含むフィードバックループを考慮すると、軸
間干渉トルクを含めて４状態変数が必要になる。まずこ
のことを説明する。図４で示す位置・速度ループ２６を
除去して、運動方程式を求めると次の式（４）となる。
【数４】 一方、位置・速度ループ２６はＰ・ＰＩ制御を行うとす
ると、トルク指令は次の式（５）となる。
ると、トルク指令は次の式（５）となる。
【数５】 式（４）（５）では、Ｔdの影響がθsとθm、θmの微分
に含まれるため、これに関する変数を式（４）（５）を
変形して除去することにより、式（６）の様に減速機構
のバネ特性と電動機を含むフィードバックループに与え
る軸間干渉トルクの影響が陽に表れる。
に含まれるため、これに関する変数を式（４）（５）を
変形して除去することにより、式（６）の様に減速機構
のバネ特性と電動機を含むフィードバックループに与え
る軸間干渉トルクの影響が陽に表れる。
【数６】 ただし、Ｔref0は位置速度ループから出力されるトルク
指令である。これより、式（６）の右辺第２、３、４、
５項を打ち消すようにＴref0をトルク指令Ｔre fに加算
すればよいことがわかる。式（２）より、
指令である。これより、式（６）の右辺第２、３、４、
５項を打ち消すようにＴref0をトルク指令Ｔre fに加算
すればよいことがわかる。式（２）より、
【数７】 となることから、非干渉化用状態変数Ｘikは、アーム加
速度とその１回微分値、２回微分値、積分値の４変数と
なる。この実施例では、関節角指令値の差分を用いてい
る。
速度とその１回微分値、２回微分値、積分値の４変数と
なる。この実施例では、関節角指令値の差分を用いてい
る。
【００１０】非干渉化ゲイン演算機構３２は、図１に示
すように、慣性行列計算機構３１から求められる干渉イ
ナーシャ４１と、制御ゲインデータ２４に格納している
式（６）に示したＫp，Ｋv，Ｋi，Ｋ，Ｊmを用いて、第
ｊ軸から第ｉ軸への非干渉化ゲイン行列Ｋijkを求め
る。図６の３軸ロボットの場合、式（８）のように求め
られる。なお、式（８）のゲイン行列要素の（）に付い
ている添字は、非干渉化の対象となっている軸を示し、
その軸の制御ゲインを使用することを表す。
すように、慣性行列計算機構３１から求められる干渉イ
ナーシャ４１と、制御ゲインデータ２４に格納している
式（６）に示したＫp，Ｋv，Ｋi，Ｋ，Ｊmを用いて、第
ｊ軸から第ｉ軸への非干渉化ゲイン行列Ｋijkを求め
る。図６の３軸ロボットの場合、式（８）のように求め
られる。なお、式（８）のゲイン行列要素の（）に付い
ている添字は、非干渉化の対象となっている軸を示し、
その軸の制御ゲインを使用することを表す。
【数８】 例えば、Ｋ12kは、第２軸の非干渉化用状態変数Ｘ2kと
乗算され、第２軸から第１軸への干渉トルクの補償トル
クを計算するゲインとなっている。第２軸から第１軸へ
の干渉トルクの補償トルクは、次の式（９）のように表
される。
乗算され、第２軸から第１軸への干渉トルクの補償トル
クを計算するゲインとなっている。第２軸から第１軸へ
の干渉トルクの補償トルクは、次の式（９）のように表
される。
【数９】 第ｉ軸について、他の軸からの干渉トルクの補償トルク
をすべて含めた非干渉化トルク指令Ｔcriは、ＫijkとＸ
ikを乗算器で乗算し、結果を加算器で加算することによ
り得られる。すなわち、
をすべて含めた非干渉化トルク指令Ｔcriは、ＫijkとＸ
ikを乗算器で乗算し、結果を加算器で加算することによ
り得られる。すなわち、
【数１０】 となる。
【００１１】図１での非干渉化用状態変数演算機構３３
で求められるアーム角加速度を含む状態変数Ｘik４３
は、各関節角指令値を微分した値を使っているため、電
動機の位置や速度検出器の分解能に影響されない利点が
あるが、そのまま使用すると位置速度ループによる遅れ
のため、非干渉化の効果が低減することから、図４に示
す関節角指令２２から他軸への干渉トルクとなるアーム
加速度５６までの伝達関数を２次の伝達関数で近似した
フィルタ３６で処理することで、少ない計算量で遅れの
問題を解決している。図１の実施例では、式（１０）の
計算のあと、２次フィルタ処理して非干渉化トルク補正
値Ｔciを求めている。これまでの説明の中では、関節角
に関節角指令値を用いているが、電動機の位置検出器の
分解能が十分であり、位置信号の差分による精度の劣化
が問題ないとされる場合には、位置信号を用いてもよ
い。
で求められるアーム角加速度を含む状態変数Ｘik４３
は、各関節角指令値を微分した値を使っているため、電
動機の位置や速度検出器の分解能に影響されない利点が
あるが、そのまま使用すると位置速度ループによる遅れ
のため、非干渉化の効果が低減することから、図４に示
す関節角指令２２から他軸への干渉トルクとなるアーム
加速度５６までの伝達関数を２次の伝達関数で近似した
フィルタ３６で処理することで、少ない計算量で遅れの
問題を解決している。図１の実施例では、式（１０）の
計算のあと、２次フィルタ処理して非干渉化トルク補正
値Ｔciを求めている。これまでの説明の中では、関節角
に関節角指令値を用いているが、電動機の位置検出器の
分解能が十分であり、位置信号の差分による精度の劣化
が問題ないとされる場合には、位置信号を用いてもよ
い。
【００１２】図２は本発明の他の実施例である。この実
施例では、非干渉化用状態変数演算機構３３に状態推定
オブザーバを使用している。この場合、状態推定のため
に、各軸毎の電動機へのトルク指令や位置検出器の位置
信号を使用することから、位置検出器の分解能の影響が
非干渉化トルク指令に表れてしまう。すなわち式（１
０）でアーム角加速度の微分値や２回微分値を求めるの
に差分を用いると、リップルが大きくなってしまい、軸
間干渉トルク補償効果が低くなるどころか、逆に加振す
ることがあるため、できるだけ差分は用いない方が望ま
しい。本実施例では、状態推定オブザーバの推定状態変
数を巧みに選択し、活用することで、アーム加速度の積
分手段、１回微分手段、２回微分手段を１回微分手段の
みにし、リップルと演算量の低減を可能にしている。
施例では、非干渉化用状態変数演算機構３３に状態推定
オブザーバを使用している。この場合、状態推定のため
に、各軸毎の電動機へのトルク指令や位置検出器の位置
信号を使用することから、位置検出器の分解能の影響が
非干渉化トルク指令に表れてしまう。すなわち式（１
０）でアーム角加速度の微分値や２回微分値を求めるの
に差分を用いると、リップルが大きくなってしまい、軸
間干渉トルク補償効果が低くなるどころか、逆に加振す
ることがあるため、できるだけ差分は用いない方が望ま
しい。本実施例では、状態推定オブザーバの推定状態変
数を巧みに選択し、活用することで、アーム加速度の積
分手段、１回微分手段、２回微分手段を１回微分手段の
みにし、リップルと演算量の低減を可能にしている。
【００１３】具体的に説明する。本実施例の状態推定オ
ブザーバで使用する運動方程式は、次の式（１１）を用
いる。
ブザーバで使用する運動方程式は、次の式（１１）を用
いる。
【数１１】 状態変数に、電動機速度ωm、電動機とアーム間のねじ
れ角θs、アーム速度ωLを選択すると状態方程式は式
（１２）のようになる。
れ角θs、アーム速度ωLを選択すると状態方程式は式
（１２）のようになる。
【数１２】 観測量を電動機速度ωmとしてオブザーバを構成する
と、得られる推定値はθsの推定値θs#とωLの推定値ω
L#である。このように構成した状態推定オブザーバによ
るθs#には、重カモーメントや軸間干渉トルクを含むそ
の他外乱によるねじれが含まれているため、式（１１）
から次のように重カモーメントによる影響のみを除去し
たアーム角加速度を得ることができる。
と、得られる推定値はθsの推定値θs#とωLの推定値ω
L#である。このように構成した状態推定オブザーバによ
るθs#には、重カモーメントや軸間干渉トルクを含むそ
の他外乱によるねじれが含まれているため、式（１１）
から次のように重カモーメントによる影響のみを除去し
たアーム角加速度を得ることができる。
【数１３】
【００１４】次に、アーム角加速度の１回微分値である
が、式（１３）の両辺を微分し、重カモーメントの時間
変動は微小と仮定することにより、アーム角速度の推定
値ωL#を使って、次のように得られる。
が、式（１３）の両辺を微分し、重カモーメントの時間
変動は微小と仮定することにより、アーム角速度の推定
値ωL#を使って、次のように得られる。
【数１４】 アーム加速度の２回微分は、式（１３）で得られたアー
ム角加速度の１回微分の結果を差分することで求める。
また、アーム角加速度の積分は、アーム角速度なので、
アーム角速度の推定値ωL#を用いる。以上から、本実施
例では、状態推定オブザーバを使用することで、アーム
加速度の積分手段、１回微分手段、２回微分手段を１回
微分手段のみにし、リップルと演算量の低減を可能にし
ていることが分かる。後は図１の実施例と同様に、非干
渉化ゲイン演算機構から得られるゲインと乗算、加算
し、非干渉化トルク指令Ｔcriを得る。図２の実施例の
場合、非干渉化トルク指令のフィルタは、１回微分によ
るリップル低減を目的にする程度で、特に問題なければ
時定数を０にしてもよい。
ム角加速度の１回微分の結果を差分することで求める。
また、アーム角加速度の積分は、アーム角速度なので、
アーム角速度の推定値ωL#を用いる。以上から、本実施
例では、状態推定オブザーバを使用することで、アーム
加速度の積分手段、１回微分手段、２回微分手段を１回
微分手段のみにし、リップルと演算量の低減を可能にし
ていることが分かる。後は図１の実施例と同様に、非干
渉化ゲイン演算機構から得られるゲインと乗算、加算
し、非干渉化トルク指令Ｔcriを得る。図２の実施例の
場合、非干渉化トルク指令のフィルタは、１回微分によ
るリップル低減を目的にする程度で、特に問題なければ
時定数を０にしてもよい。
【００１５】
【発明の効果】以上説明したように本発明では、少なく
とも２本以上のアームが関節部を介し、シリアルに結合
された構造を持つロボットを制御する多軸ロボット制御
装置において、各アームの角度と、各アームの質量、長
さ等の動力学パラメータとから慣性行列を作成する慣性
行列計算機構と、各アームの加速度を含む非干渉化用状
態変数を演算する演算機構と、これら状態変数に乗算す
る非干渉化ゲインを演算する演算機構と、前記非干渉化
用状態変数と前記非干渉化ゲインとを乗算及び加算する
乗算部及び加算部と、その計算の結果得られる非干渉化
トルク指令から高調波リップル分を除去するためのフィ
ルタ処理部を有することで、安定した軌跡精度を得るこ
とのできるロボットの非干渉化制御装置を得ることがで
きる。
とも２本以上のアームが関節部を介し、シリアルに結合
された構造を持つロボットを制御する多軸ロボット制御
装置において、各アームの角度と、各アームの質量、長
さ等の動力学パラメータとから慣性行列を作成する慣性
行列計算機構と、各アームの加速度を含む非干渉化用状
態変数を演算する演算機構と、これら状態変数に乗算す
る非干渉化ゲインを演算する演算機構と、前記非干渉化
用状態変数と前記非干渉化ゲインとを乗算及び加算する
乗算部及び加算部と、その計算の結果得られる非干渉化
トルク指令から高調波リップル分を除去するためのフィ
ルタ処理部を有することで、安定した軌跡精度を得るこ
とのできるロボットの非干渉化制御装置を得ることがで
きる。
【図１】 本発明の請求項３の非干渉化制御装置を含む
ロボット制御装置の実施例を示すブロック図である。
ロボット制御装置の実施例を示すブロック図である。
【図２】 本発明の請求項４の非干渉化制御装置を含む
ロボット制御装置の実施例を示すブロック図である。
ロボット制御装置の実施例を示すブロック図である。
【図３】 産業用ロボットのある１つのアームの構造を
表した模式図である。
表した模式図である。
【図４】 産業用ロボットのある１つのアームを含んだ
制御系ブロック図である。
制御系ブロック図である。
【図５】 ６軸産業用ロボットの関節構成図である。
【図６】 ３軸ロボットの関節構成図である。
１ ロボット、２ 既存のディジタル制御装置、２ａ
ディジタル制御装置の上位側、２ｂ ディジタル制御装
置の下位側、２１ ロボットの手先目標値、２２関節角
指令値、２３ 動力学デー夕、２４ 制御ゲインデー
タ、２５１ 第１軸目のフィードバック制御部、２５２
第２軸目のフィードバック制御部、２５ｎ 第ｎ軸目
のフィードバック制御部、２６ 位置・速度ループ、２
７ トルク指令の加算器、２８ 電動機の位置検出器か
らの位置信号、３ 非干渉化制御装置、３１ 慣性行列
計算機構、３２ 非干渉化ゲイン演算機構、３３ 非干
渉化用状態変数演算機構、３４ 乗算器、３５ 加算
器、３６ フィル夕、４１ 慣性行列要素Ｍij、４２
非干渉化ゲイン行列要素Ｋijk、４３ 非干渉化用状態
変数Ｘik、４４ 非干渉化トルク指令Ｔcri、４５ 非
干渉化トルク補正値Ｔci、５０ 外乱トルク、５１ 電
動機速度、５２ ねじれ速度、５３ 駆動トルク、５４
軸間干渉トルク、５５ アーム速度、５６ アーム加
速度、１０１ 電動機、１０２ 減速機構、１０３ ロ
ボットアーム
ディジタル制御装置の上位側、２ｂ ディジタル制御装
置の下位側、２１ ロボットの手先目標値、２２関節角
指令値、２３ 動力学デー夕、２４ 制御ゲインデー
タ、２５１ 第１軸目のフィードバック制御部、２５２
第２軸目のフィードバック制御部、２５ｎ 第ｎ軸目
のフィードバック制御部、２６ 位置・速度ループ、２
７ トルク指令の加算器、２８ 電動機の位置検出器か
らの位置信号、３ 非干渉化制御装置、３１ 慣性行列
計算機構、３２ 非干渉化ゲイン演算機構、３３ 非干
渉化用状態変数演算機構、３４ 乗算器、３５ 加算
器、３６ フィル夕、４１ 慣性行列要素Ｍij、４２
非干渉化ゲイン行列要素Ｋijk、４３ 非干渉化用状態
変数Ｘik、４４ 非干渉化トルク指令Ｔcri、４５ 非
干渉化トルク補正値Ｔci、５０ 外乱トルク、５１ 電
動機速度、５２ ねじれ速度、５３ 駆動トルク、５４
軸間干渉トルク、５５ アーム速度、５６ アーム加
速度、１０１ 電動機、１０２ 減速機構、１０３ ロ
ボットアーム
Claims (4)
- 【請求項１】 少なくとも２本以上のアームが関節部を
介し、シリアルに結合された構造を持つロボットを制御
する多軸ロボット制御装置において、各アームの角度
と、各アームの質量、長さ等の動力学パラメータとから
慣性行列を作成する慣性行列計算機構と、各アームの加
速度を含む非干渉化用状態変数を演算する演算機構と、
これら状態変数に乗算する非干渉化ゲインを演算する演
算機構と、前記非干渉化用状態変数と前記非干渉化ゲイ
ンとを乗算及び加算する乗算部及び加算部と、その計算
の結果得られる非干渉化トルク指令から高調波リップル
分を除去するためのフィルタ処理部を有することを特徴
とするロボットの非干渉化制御装置。 - 【請求項２】 前記各アームの加速度を含む非干渉化用
状態変数の演算機構は、各軸アームヘの関節角指令値を
用いたことを特徴とする請求項１記載のロボットの非干
渉化制御装置。 - 【請求項３】 前記非干渉化トルク指令のフィルタ処理
部は、２次フィルタを用いたことを特徴とする請求項１
または２記載のロボットの非干渉化制御装置。 - 【請求項４】 前記各アームの加速度を含む非干渉化用
状態変数の演算機構は、状態推定オブザーバを使用した
ことを特徴とする請求項１記載のロボットの非干渉化制
御装置。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2551497A JPH10217173A (ja) | 1997-02-07 | 1997-02-07 | ロボットの非干渉化制御装置 |
| PCT/JP1998/000494 WO1998034762A1 (fr) | 1997-02-07 | 1998-02-05 | Dispositif de commande non interactif pour robots |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2551497A JPH10217173A (ja) | 1997-02-07 | 1997-02-07 | ロボットの非干渉化制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH10217173A true JPH10217173A (ja) | 1998-08-18 |
Family
ID=12168179
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2551497A Pending JPH10217173A (ja) | 1997-02-07 | 1997-02-07 | ロボットの非干渉化制御装置 |
Country Status (2)
| Country | Link |
|---|---|
| JP (1) | JPH10217173A (ja) |
| WO (1) | WO1998034762A1 (ja) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2013180222A1 (ja) | 2012-05-30 | 2013-12-05 | 株式会社神戸製鋼所 | 多関節ロボットの軌跡制御装置および制御方法 |
| WO2013180223A1 (ja) * | 2012-05-30 | 2013-12-05 | 株式会社神戸製鋼所 | 多関節ロボットの弾性変形補償制御装置および制御方法 |
| JP2013248683A (ja) * | 2012-05-30 | 2013-12-12 | Kobe Steel Ltd | 多関節ロボットの弾性変形補償制御装置および制御方法 |
| WO2014126218A1 (ja) * | 2013-02-15 | 2014-08-21 | 株式会社神戸製鋼所 | 多関節ロボットの弾性変形補償制御装置 |
| CN108803350A (zh) * | 2018-08-16 | 2018-11-13 | 居鹤华 | 基于轴不变量的动基座多轴机器人动力学建模与解算方法 |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2631783C1 (ru) * | 2016-06-22 | 2017-09-26 | Федеральное государственное автономное образовательное учреждение высшего образования "Дальневосточный федеральный университет" (ДВФУ) | Самонастраивающийся электропривод манипулятора |
| RU2631784C1 (ru) * | 2016-06-22 | 2017-09-26 | Федеральное государственное автономное образовательное учреждение высшего образования "Дальневосточный федеральный университет" (ДВФУ) | Самонастраивающийся электропривод манипулятора |
| RU2705739C1 (ru) * | 2018-06-29 | 2019-11-11 | Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) | Самонастраивающийся электропривод манипулятора |
| RU2705734C1 (ru) * | 2018-06-29 | 2019-11-11 | Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) | Самонастраивающийся электропривод манипулятора |
| RU2705737C1 (ru) * | 2018-06-29 | 2019-11-11 | Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) | Самонастраивающийся электропривод манипулятора |
| RU2706079C1 (ru) * | 2018-09-14 | 2019-11-13 | Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) | Самонастраивающийся электропривод манипулятора |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61273611A (ja) * | 1985-05-30 | 1986-12-03 | Matsushita Electric Ind Co Ltd | 産業用ロボツト |
| JPS63314606A (ja) * | 1987-06-18 | 1988-12-22 | Fanuc Ltd | 多関節ロボットの制御装置 |
| JPH0863213A (ja) * | 1994-08-18 | 1996-03-08 | Fanuc Ltd | 干渉トルクの計算処理方法 |
| JPH08106312A (ja) * | 1994-10-05 | 1996-04-23 | Fanuc Ltd | 干渉トルクの計算処理方法 |
-
1997
- 1997-02-07 JP JP2551497A patent/JPH10217173A/ja active Pending
-
1998
- 1998-02-05 WO PCT/JP1998/000494 patent/WO1998034762A1/ja active Application Filing
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9254569B2 (en) | 2012-05-30 | 2016-02-09 | Kobe Steel, Ltd. | Trajectory control device and control method for articulated robot |
| WO2013180223A1 (ja) * | 2012-05-30 | 2013-12-05 | 株式会社神戸製鋼所 | 多関節ロボットの弾性変形補償制御装置および制御方法 |
| JP2013248682A (ja) * | 2012-05-30 | 2013-12-12 | Kobe Steel Ltd | 多関節ロボットの軌跡制御装置および制御方法 |
| JP2013248683A (ja) * | 2012-05-30 | 2013-12-12 | Kobe Steel Ltd | 多関節ロボットの弾性変形補償制御装置および制御方法 |
| JP2013248681A (ja) * | 2012-05-30 | 2013-12-12 | Kobe Steel Ltd | 多関節ロボットの弾性変形補償制御装置および制御方法 |
| US9505131B2 (en) | 2012-05-30 | 2016-11-29 | Kobe Steel, Ltd. | Elastic-deformation-compensation control device and control method for articulated robot |
| WO2013180222A1 (ja) | 2012-05-30 | 2013-12-05 | 株式会社神戸製鋼所 | 多関節ロボットの軌跡制御装置および制御方法 |
| KR20150006016A (ko) | 2012-05-30 | 2015-01-15 | 가부시키가이샤 고베 세이코쇼 | 다관절 로봇의 궤적 제어 장치 및 제어 방법 |
| CN104334323A (zh) * | 2012-05-30 | 2015-02-04 | 株式会社神户制钢所 | 多关节机器人的轨迹控制装置以及控制方法 |
| JP2014155982A (ja) * | 2013-02-15 | 2014-08-28 | Kobe Steel Ltd | 多関節ロボットの弾性変形補償制御装置 |
| CN104981326A (zh) * | 2013-02-15 | 2015-10-14 | 株式会社神户制钢所 | 多关节机器人的弹性变形补偿控制装置 |
| WO2014126218A1 (ja) * | 2013-02-15 | 2014-08-21 | 株式会社神戸製鋼所 | 多関節ロボットの弾性変形補償制御装置 |
| US9597800B2 (en) | 2013-02-15 | 2017-03-21 | Kobe Steel, Ltd. | Elastic deformation compensation control device for articulated robot |
| CN108803350A (zh) * | 2018-08-16 | 2018-11-13 | 居鹤华 | 基于轴不变量的动基座多轴机器人动力学建模与解算方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| WO1998034762A1 (fr) | 1998-08-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3324298B2 (ja) | マニピュレータの制御装置 | |
| JP3189865B2 (ja) | 機械振動検出装置および制振制御装置 | |
| JP2003009566A (ja) | 電動モータを用いた車両の制振制御装置 | |
| JP4685071B2 (ja) | モータ制御装置及びモータ制御方法 | |
| JP3981773B2 (ja) | ロボット制御装置 | |
| JPH10217173A (ja) | ロボットの非干渉化制御装置 | |
| KR19980083174A (ko) | 공진계의 진동억제 제어방법 및 제어장치 | |
| CN113799136A (zh) | 一种基于全状态反馈的机器人关节高精度控制系统及方法 | |
| JPH10309684A (ja) | マニピュレータのコンプライアンス制御方式 | |
| JP3081518B2 (ja) | ロボットの剛性同定方法及びその装置 | |
| JPH09212203A (ja) | ロボット制御装置 | |
| JP3220589B2 (ja) | メカニカルシステムの制御装置 | |
| JP2005269833A (ja) | 車両用制振制御装置および車両用制振制御方法 | |
| KR101053205B1 (ko) | 백래쉬 보상 기능을 갖는 모터 제어 장치 | |
| JP3856215B2 (ja) | 速度制御装置 | |
| JPH08278821A (ja) | サーボ制御系の制振方法 | |
| JP2838578B2 (ja) | モータ制御装置、外乱負荷トルク推定装置 | |
| CN114310874A (zh) | 协作机器人关节力矩控制方法、系统及计算机设备 | |
| JPH02109749A (ja) | 車両駆動系制御装置および制御方法 | |
| JP4038659B2 (ja) | サーボ制御装置 | |
| JP2869281B2 (ja) | モータ駆動系の制御装置 | |
| JP3972155B2 (ja) | モータ制御装置 | |
| JP3307564B2 (ja) | サーボ系の駆動制御装置 | |
| JPH07120212B2 (ja) | 産業用ロボットの制御装置 | |
| CN113767565B (zh) | 马达控制系统、马达控制方法以及程序 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A131 | Notification of reasons for refusal |
Effective date: 20060519 Free format text: JAPANESE INTERMEDIATE CODE: A131 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060711 |
|
| A521 | Written amendment |
Effective date: 20060803 Free format text: JAPANESE INTERMEDIATE CODE: A523 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060811 |
|
| A131 | Notification of reasons for refusal |
Effective date: 20061222 Free format text: JAPANESE INTERMEDIATE CODE: A131 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070126 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070316 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20070706 |