JP2002331477A - ロボット制御方法および制御装置 - Google Patents

Abstract

(57)【要約】 【課題】他軸の動きによらず、各軸の応答性を同じにし
て、マニピュレータ先端の軌跡誤差を少なくし、また、
制御パラメータを他軸の動きに関係なく独立に設計でき
るようにして、制御系の設計を容易にすることができる
ロボット制御方法および制御装置を提供する。 【解決手段】指令値を入力し、アーム１５の駆動に必要
なトルクを設定値として、設定値に対するアーム１５の
姿勢変化に伴い、自軸のノミナルな慣性モーメントのト
ルクを除いた、動力学のトルク成分をトルク変動値と
し、これを補償する補償トルク算出部１を備え、補償ト
ルク算出部１の出力信号を電流制御部２３に入力する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ロボットの動作制
御に係る教示装置およびその制御方法等に適用されるロ
ボット制御方法および制御装置に関するものである。

【０００２】

【従来の技術】図２は従来のロボット制御装置の１軸分
の制御構成を示すブロック図である。図において、２慣
性系１６はロボット制御装置の制御対象で、モータ１０
とアーム１５（以下、負荷という）を減速機で結合した
ものからなる。ここで、モータ１０のＪｍは慣性を示
し、Ｄｍは粘性係数を示す。同様に、アーム１５のＪＬ
は慣性、ＤＬは粘性係数を示す。減速機は他の要素（モ
ータ、アーム）に比べて弾性的であり、係数１４はその
弾性定数である。また係数１１、１２は減速機の減速比
である。図中でωＭはモータ速度、ωＬは負荷速度、τ
Ｌは負荷トルクを示す。制御部における、位置制御部２
１は位置指令とモータ位置θＭとを入力として位置制御
を行い、速度指令値を出力する部分である。速度制御部
２２は上記速度指令値とモータ速度とを入力として速度
制御を行い、電流指令値を出力する。電流制御部２３は
上記電流指令値にもとづきモータ１０に指令通りの電流
を流すように制御を行う部分である。なお、係数２４は
モータ１０のトルク定数である。１３は積分要素、２０
は積分器、４３〜４５は減算部である。

【０００３】ロボットは複数軸からなるので、他軸から
の干渉力、すなわち、他軸の慣性に対して速度を与える
ための加速トルク、遠心力、コリオリカ、重力が働く。
上記での負荷トルクτＬはこの他軸からの干渉力を示
し、これは減速機を通じてモータ１０に外乱トルクとし
て作用する。外乱トルク算出部３０はこれを補償するた
めのもので、モータ電流とモータ速度とからモータ１０
の外乱トルクを推定算出して、それを補償量として電流
指令値の形で出力する部分である。外乱トルク算出部３
ｄからの補償量は加算部４２で速度制御部２２からの出
力に加算されて電流制御部２３の入力となる。

【０００４】このような構成によって、他軸からの干渉
力を排して各軸独立した制御が行える。

【０００５】

【発明が解決しようとする課題】ロボットのマニピュレ
ータ先端の軌跡を精度よく制御するためには、各軸の位
置指令に対する応答性を同じにする必要がある。他軸か
らの干渉力が十分に補償されていれば、位置制御部２
１、速度制御部２２、電流制御部２３の応答性を全軸同
じになるように設計することによって、これが可能とな
る。

【０００６】しかしながら、上記従来技術では、位置指
令に対する結果であるところのモータ速度から外乱トル
クを算出しているので、真の外乱トルク値が得られるま
での算出遅れが生じる。また、他軸の動き方、位置によ
ってその干渉力は様々に変化するので、外乱トルクの算
出遅れの程度は一定していない。したがって、各軸の位
置指令に対する応答性を同じにすることはできない。ま
た、この外乱トルク算出の軸間でのばらつきは、２慣性
系１６に対する振動抑制のための制御系を設計する際に
もその設計見通しを立てにくくする。これらのことか
ら、ロボットのマニピュレータ先端の軌跡精度を高める
のが困難である。

【０００７】本発明は、上記課題を解決するものであっ
て、他軸の動きによらず、各軸の応答性を同じにして、
マニピュレータ先端の軌跡誤差を少なくし、また、制御
パラメータを他軸の動きに関係なく独立に設計できるよ
うにして、制御系の設計を容易にすることができるロボ
ット制御方法および制御装置を提供することを目的とす
るものである。

【０００８】

【課題を解決するための手段】請求項１記載のロボット
制御方法は、ロボットのマニピュレータを駆動するモー
タの動作を指示する指令値を入力し、指令値に基づいて
位置を制御する信号を出力する第１ステップと、第１ス
テップで出力された信号に基づいてモータの回転速度を
制御する信号を出力する第２ステップと、第２ステップ
で出力された信号に基づいてモータを駆動する電流を制
御する第３ステップと、第３ステップで出力された電流
に基づいてモータを駆動する第４ステップと、第４ステ
ップで駆動されたモータの速度を検出し、その検出値を
位置信号に変換して第１ステップにフィードバック入力
し、検出値を第２ステップおよび第３ステップにフィー
ドバック入力するフィードバックステップと、指令値を
入力し、マニピュレータの駆動に必要なトルクを設定値
として、設定値に対するマニピュレータの姿勢変化に伴
い、自軸のノミナルな慣性モーメントのトルクを除い
た、動力学のトルク成分をトルク変動値とし、これを補
償する補償トルクを算出する補償ステップとを含み、補
償ステップで算出される補償トルクに応じた出力信号を
第３ステップに入力することを特徴とするものである。

【０００９】請求項１記載のロボット制御方法によれ
ば、マニピュレータの動きによらず、各関節軸の応答性
を同じにして、マニピュレータ先端の軌跡誤差を少なく
し、また、制御パラメタをマニピュレータの動きに関係
なく設定できるようにして、制御系の設計を容易にする
ことができる。

【００１０】請求項２記載のロボット制御装置は、ロボ
ットのマニピュレータを駆動するモータと、モータの動
作を指示する指令値を入力する位置制御部と、位置制御
部からの信号を入力し、モータの速度を制御する速度制
御部と、速度制御部からの信号を入力し、モータへ供給
する電流を制御する電流制御部と、モータの状態を検出
し、電流制御部と速度制御部と位置制御部にフィードバ
ックするフィードバック部と、指令値を入力し、マニピ
ュレータの駆動に必要なトルクを設定値として、設定値
に対するマニピュレータの姿勢変化に伴い、自軸のノミ
ナルな慣性モーメントのトルクを除いた、動力学のトル
ク成分をトルク変動値とし、これを補償する補償トルク
算出部とを備え、補償トルク算出部の出力信号を電流制
御部に入力するものである。

【００１１】請求項２記載のロボット制御装置によれ
ば、請求項１と同様な効果がある。

【００１２】請求項３記載のロボット制御装置は、請求
項２において、補償トルク算出部が、指令値を入力して
位置指令値と位置指令値から負荷速度までの伝達関数を
基に負荷の位置、速度および加速度を算出する負荷状態
量算出部と、負荷の位置、速度および加速度を入力して
他軸からの干渉力を算出する逆動力学計算部と、干渉力
をこれに見合った電流指令値に変換し電流制御部に入力
する変換部を有するものである。

【００１３】請求項３記載のロボット制御装置によれ
ば、請求項２と同様な効果がある。

【００１４】

【発明の実施の形態】次に本発明の一実施の形態を説明
する。図１は、本発明の構成を示すブロック図である。
従来技術の構成で説明したのと同じ要素については、同
じ符号を付して説明を省略し、異なる部分のみ説明す
る。

【００１５】従来技術の構成と異なるのは、指令値を入
力し、マニピュレータの駆動に必要なトルクを設定値と
して、設定値に対するマニピュレータの姿勢変化に伴
い、自軸のノミナルな慣性モーメントのトルクを除い
た、動力学のトルク成分をトルク変動値とし、これを補
償する補償トルク算出部１を付加した点である。

【００１６】補償トルク算出部１は、負荷状態量算出部
２、逆動力学計算部３、変換部４からなる。

【００１７】負荷状態量算出部２は、位置指令を入力と
して、位置指令から負荷速度ωＬまでの伝達関数をもと
に、負荷の位置、速度、加速度を算出する部分である。
逆動力学計算部３は、負荷状態量算出部２からの、負荷
の位置、速度、加速度を入力として、他軸からの干渉力
τＬを算出する部分である。変換部４は、負荷状態量算
出部２が算出した他軸からの干渉力τＬを、これに見合
った電流指令値に変換する部分である。変換部４からの
電流指令値は加算部５で速度制御部２２からの電流指令
値に加算されて、電流制御部２３の入力となる。

【００１８】以下、逆動力学計算部３について説明す
る。

【００１９】ロボットマニピュレータの動力学方程式
は、〔数１〕の式（１）のように表記される。式（１）
の右辺第１項は角加速度によるトルクを表し、右辺第２
項目は動摩擦によるトルクを表している。また、右辺第
３項は、重力項・コリオリ項をまとめた項目である。こ
れらの慣性モーメント及び重力・コリオリ項は、関節角
度と関節角速度によって代数式で一意に決定できる。動
摩擦に関しては、一定値であると見ることが出来る。

【００２０】

【数１】

【００２１】これらの行列の計算を行って、動力学トル
クを計算する。

【００２２】ここで、各軸の制御系を考えると、ノミナ
ルの慣性モーメントに対して自軸の角加速度の影響だけ
を考えて制御系を設計できるようにするため、他の軸の
影響はすべて負荷トルク入力と見るようにする。

【００２３】そのために、第１軸目（ＲＴ軸）で代表し
て示せば、負荷側の駆動トルクは〔数２〕の式（３）、
（４）のように書くことが出来る。

【００２４】

【数２】

【００２５】式（３）の右辺第３項、すなわち式（４）
が第１軸目に入力される負荷トルクになる。この計算を
各軸ごとに行って、各軸に入力される動力学トルクを、
自軸の回転に必要なトルクとそれ以外の負荷トルクに分
解する事が出来る。その結果、負荷トルクに相当する部
分をまとめると、〔数３〕の式（５）、（６）のように
なる。

【００２６】

【数３】

【００２７】Ｈの対角項（慣性モーメント）から、各軸
のノミナルの慣性モーメント値（当該軸の負荷の慣性モ
ーメント値）を差し引いて、動力学によって変化した慣
性モーメント（他軸の慣性モーメント）の成分も補償ト
ルクとしてフィードバックする。これによって、ノミナ
ル慣性分の負荷トルクを除外した負荷トルクを計算でき
るようにする。

【００２８】なお、上記の式（５）で示した動力学計算
には、負荷側の位置・速度・加速度が必要となるが、こ
れは先に述べたように、負荷状態量算出部２で、関節位
置の位置指令と位置指令から負荷速度までの伝達関数を
用いて算出する。

【００２９】上記式（５）にもとづいて逆動力学計算部
３で算出された動力学トルクは、電流指令値の形で電流
補償入力として与えて、負荷トルクを補償する。もちろ
ん、電流でなくとも直接的にトルクとして与えることが
できるのであれば、それでも構わない。

【００３０】電流指令値として補償する場合は、以下の
ようになる。

【００３１】負荷トルクに対する〔数４〕の負荷速度の
応答（式（７））と電流指令に対する負荷速度の応答
（式（８））を求める。

【００３２】

【数４】

【００３３】ここで、ωＬｔ、ωＬｉは負荷速度であ
り、τＬは負荷トルク、Ｉｃｍｐは電流指令値である。

【００３４】式（７）、（８）について〔数５〕の式
（９）のようにおくと、式（１１）が導かれる。

【００３５】

【数５】

【００３６】式（１１）の右辺が負荷トルクを電流指令
に変換する伝達関数である。なお、表１に上記式中の記
号の意味を示す。

【００３７】

【表１】

【００３８】つぎに、この発明のロボット制御方法は、
上記ロボット制御装置を用いて、第１ステップは、ロボ
ットのマニピュレータ（１５）を駆動するモータ１０の
動作を指示する指令値を入力し、指令値に基づいて位置
を制御する信号を出力する。第２ステップは、第１ステ
ップで出力された信号に基づいてモータの回転速度を制
御する信号を出力する。第３ステップは、第２ステップ
で出力された信号に基づいてモータ１０を駆動する電流
を制御する。第４ステップは、第３ステップで出力され
た電流に基づいてモータ１０を駆動する。フィードバッ
クステップは、第４ステップで駆動されたモータ１０の
速度を検出し、その検出値を位置信号に変換して第１ス
テップにフィードバック入力し、上記検出値を第２ステ
ップおよび第３ステップにフィードバック入力する。補
償ステップは、指令値を入力し、マニピュレータの駆動
に必要なトルクを設定値として、設定値に対するマニピ
ュレータの姿勢変化に伴い、自軸のノミナルな慣性モー
メントのトルクを除いた、動力学のトルク成分をトルク
変動値とし、これを補償する補償トルクを算出し、補償
ステップで算出される補償トルクに応じた出力信号を第
３ステップに入力するものである。

【００３９】

【発明の効果】請求項１記載のロボット制御方法によれ
ば、マニピュレータの動きによらず、各関節軸の応答性
を同じにして、マニピュレータ先端の軌跡誤差を少なく
し、また、制御パラメタをマニピュレータの動きに関係
なく設定できるようにして、制御系の設計を容易にする
ことができる。

【００４０】請求項２および請求項３記載のロボット制
御装置によれば、請求項１と同様な効果がある。

【図面の簡単な説明】

【図１】本発明の一実施の形態におけるロボット制御装
置のブロック図である。

【図２】従来のロボット制御装置のブロック図である。

【符号の説明】

１ 補償トルク算出部 ２ 負荷状態量算出部 ３ 逆動力学計算部 ４ 変換部 ５ 加算部 １０ モータ １１、１２ 係数（減速比） １３ 積分要素 １４ 係数（弾性係数） １５ 負荷（アーム） １６ ２慣性系 ２０ 積分器 ２１ 位置制御部 ２２ 速度制御部 ２３ 電流制御部 ２４ 係数（トルク定数） ３０ 外乱トルク算出部 ４２ 加算部 ４３〜４５ 減算部

───────────────────────────────────────────────────── フロントページの続き (72)発明者 中塚 隆 大阪府門真市大字門真1006番地 松下電器 産業株式会社内 (72)発明者 大石 潔 新潟県長岡市学校町一丁目３番４号 (72)発明者 宮崎 敏昌 新潟県長岡市悠久町１−１ 高専宿舎39号 Ｆターム(参考） 3C007 KS21 KS37 LT13 LV18 LV19 LV23 LW01 5H269 AB33 CC09 EE03 EE05 GG03

Claims (3)

【特許請求の範囲】
1. 【請求項１】 ロボットのマニピュレータを駆動するモ
   ータの動作を指示する指令値を入力し、指令値に基づい
   て位置を制御する信号を出力する第１ステップと、第１
   ステップで出力された信号に基づいてモータの回転速度
   を制御する信号を出力する第２ステップと、第２ステッ
   プで出力された信号に基づいてモータを駆動する電流を
   制御する第３ステップと、第３ステップで出力された電
   流に基づいてモータを駆動する第４ステップと、第４ス
   テップで駆動されたモータの速度を検出し、その検出値
   を位置信号に変換して第１ステップにフィードバック入
   力し、前記検出値を前記第２ステップおよび前記第３ス
   テップにフィードバック入力するフィードバックステッ
   プと、前記指令値を入力し、前記マニピュレータの駆動
   に必要なトルクを設定値として、設定値に対するマニピ
   ュレータの姿勢変化に伴い、自軸のノミナルな慣性モー
   メントのトルクを除いた、動力学のトルク成分をトルク
   変動値とし、これを補償する補償トルクを算出する補償
   ステップとを含み、前記補償ステップで算出される補償
   トルクに応じた出力信号を前記第３ステップに入力する
   ことを特徴とするロボット制御方法。
2. 【請求項２】 ロボットのマニピュレータを駆動するモ
   ータと、モータの動作を指示する指令値を入力する位置
   制御部と、前記位置制御部からの信号を入力し、前記モ
   ータの速度を制御する速度制御部と、前記速度制御部か
   らの信号を入力し、前記モータへ供給する電流を制御す
   る電流制御部と、前記モータの状態を検出し、前記電流
   制御部と前記速度制御部と前記位置制御部にフィードバ
   ックするフィードバック部と、前記指令値を入力し、前
   記マニピュレータの駆動に必要なトルクを設定値とし
   て、前記設定値に対する前記マニピュレータの姿勢変化
   に伴い、自軸のノミナルな慣性モーメントのトルクを除
   いた、動力学のトルク成分をトルク変動値とし、これを
   補償する補償トルク算出部とを備え、前記補償トルク算
   出部の出力信号を前記電流制御部に入力するロボット制
   御装置。
3. 【請求項３】 補償トルク算出部は、指令値を入力して
   位置指令値と前記位置指令値から負荷速度までの伝達関
   数を基に負荷の位置、速度および加速度を算出する負荷
   状態量算出部と、前記負荷の位置、速度および加速度を
   入力して他軸からの干渉力を算出する逆動力学計算部
   と、前記干渉力をこれに見合った電流指令値に変換し前
   記電流制御部に入力する変換部を有する請求項２記載の
   ロボット制御装置。