JP4008207B2

JP4008207B2 - ロボット制御方法および制御装置

Info

Publication number: JP4008207B2
Application number: JP2001135330A
Authority: JP
Inventors: 敦実橋本; 隆中塚; 潔大石; 敏昌宮崎
Original assignee: Panasonic Corp; Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Corp; Panasonic Holdings Corp
Priority date: 2001-05-02
Filing date: 2001-05-02
Publication date: 2007-11-14
Anticipated expiration: 2021-05-02
Also published as: JP2002331477A

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットの動作制御に係る教示装置およびその制御方法等に適用されるロボット制御方法および制御装置に関するものである。
【０００２】
【従来の技術】
図２は従来のロボット制御装置の１軸分の制御構成を示すブロック図である。図において、２慣性系１６はロボット制御装置の制御対象で、モータ１０とアーム１５（以下、負荷という）を減速機で結合したものからなる。ここで、モータ１０のＪｍは慣性を示し、Ｄｍは粘性係数を示す。同様に、アーム１５のＪＬは慣性、ＤＬは粘性係数を示す。減速機は他の要素（モータ、アーム）に比べて弾性的であり、係数１４はその弾性定数である。また係数１１、１２は減速機の減速比である。図中でωＭはモータ速度、ωＬは負荷速度、τＬは負荷トルクを示す。制御部における、位置制御部２１は位置指令とモータ位置θＭとを入力として位置制御を行い、速度指令値を出力する部分である。速度制御部２２は上記速度指令値とモータ速度とを入力として速度制御を行い、電流指令値を出力する。電流制御部２３は上記電流指令値にもとづきモータ１０に指令通りの電流を流すように制御を行う部分である。なお、係数２４はモータ１０のトルク定数である。１３は積分要素、２０は積分器、４３〜４５は減算部である。
【０００３】
ロボットは複数軸からなるので、他軸からの干渉力、すなわち、他軸の慣性に対して速度を与えるための加速トルク、遠心力、コリオリカ、重力が働く。上記での負荷トルクτＬはこの他軸からの干渉力を示し、これは減速機を通じてモータ１０に外乱トルクとして作用する。外乱トルク算出部３０はこれを補償するためのもので、モータ電流とモータ速度とからモータ１０の外乱トルクを推定算出して、それを補償量として電流指令値の形で出力する部分である。外乱トルク算出部３ｄからの補償量は加算部４２で速度制御部２２からの出力に加算されて電流制御部２３の入力となる。
【０００４】
このような構成によって、他軸からの干渉力を排して各軸独立した制御が行える。
【０００５】
【発明が解決しようとする課題】
ロボットのマニピュレータ先端の軌跡を精度よく制御するためには、各軸の位置指令に対する応答性を同じにする必要がある。他軸からの干渉力が十分に補償されていれば、位置制御部２１、速度制御部２２、電流制御部２３の応答性を全軸同じになるように設計することによって、これが可能となる。
【０００６】
しかしながら、上記従来技術では、位置指令に対する結果であるところのモータ速度から外乱トルクを算出しているので、真の外乱トルク値が得られるまでの算出遅れが生じる。また、他軸の動き方、位置によってその干渉力は様々に変化するので、外乱トルクの算出遅れの程度は一定していない。したがって、各軸の位置指令に対する応答性を同じにすることはできない。また、この外乱トルク算出の軸間でのばらつきは、２慣性系１６に対する振動抑制のための制御系を設計する際にもその設計見通しを立てにくくする。これらのことから、ロボットのマニピュレータ先端の軌跡精度を高めるのが困難である。
【０００７】
本発明は、上記課題を解決するものであって、他軸の動きによらず、各軸の応答性を同じにして、マニピュレータ先端の軌跡誤差を少なくし、また、制御パラメータを他軸の動きに関係なく独立に設計できるようにして、制御系の設計を容易にすることができるロボット制御方法および制御装置を提供することを目的とするものである。
【０００８】
【課題を解決するための手段】
請求項１記載のロボット制御方法は、ロボットのマニピュレータを駆動するモータの動作を指示する指令値を入力し、指令値に基づいて位置を制御する信号を出力する第１ステップと、第１ステップで出力された信号に基づいてモータの回転速度を制御する信号を出力する第２ステップと、第２ステップで出力された信号に基づいてモータを駆動する電流を制御する第３ステップと、第３ステップで出力された電流に基づいてモータを駆動する第４ステップと、第４ステップで駆動されたモータの速度を検出し、その検出値を位置信号に変換して第１ステップにフィードバック入力し、前記検出値を前記第２ステップにフィードバック入力し、モータを駆動する電流を検出し第３ステップに入力するフィードバックステップと、前記指令値を入力し、前記マニピュレータの駆動に必要なトルクを設定値として、設定値に対するマニピュレータの姿勢変化に伴い、自軸のノミナルな慣性モーメントのトルクを除いた、動力学のトルク成分をトルク変動値とし、これを補償する補償トルクを算出する補償ステップとを含み、前記補償ステップで算出される補償トルクに応じた出力信号を前記第３ステップに入力するロボット制御方法であって、前記補償ステップは、指令値を入力して位置指令値と、前記第１ステップと前記第２ステップと前記第３ステップと前記第４ステップと前記モータと前記フィードバックステップからなるフィードバック制御ループと前記モータで駆動される減速機と前記減速機に接続される負荷からなる１軸分の関節軸とで構成するシステムにおける前記位置指令値から負荷速度までの伝達関数を基に前記負荷の位置、速度および加速度を算出し、前記負荷の位置、速度および加速度を入力して他軸からの干渉力を算出し、前記干渉力をこれに見合った電流指令値に変換し前記電流制御部に入力することを特徴とするものである。
【０００９】
請求項１記載のロボット制御方法によれば、マニピュレータの動きによらず、各関節軸の応答性を同じにして、マニピュレータ先端の軌跡誤差を少なくし、また、制御パラメタをマニピュレータの動きに関係なく設定できるようにして、制御系の設計を容易にすることができる。
【００１０】
請求項２記載のロボット制御装置は、ロボットのマニピュレータを駆動するモータと、モータの動作を指示する指令値を入力する位置制御部と、前記位置制御部からの信号を入力し、前記モータの速度を制御する速度制御部と、前記速度制御部からの信号を入力し、前記モータへ供給する電流を制御する電流制御部と、前記モータの速度と位置と前記モータへ供給する電流を検出し、前記モータ速度を前記速度制御部に、前記モータ位置を前記位置制御部に、前記モータへ供給する電流を前記電流制御部にフィードバックするフィードバック部と、前記指令値を入力し、前記マニピュレータの駆動に必要なトルクを設定値として、前記設定値に対する前記マニピュレータの姿勢変化に伴い、自軸のノミナルな慣性モーメントのトルクを除いた、動力学のトルク成分をトルク変動値とし、これを補償する補償トルク算出部とを備え、前記補償トルク算出部の出力信号を前記電流制御部に入力するロボット制御装置であって、前記補償トルク算出部は、指令値を入力して位置指令値と、前記位置制御部と前記速度制御部と前記電流制御部と前記モータと前記フィードバック部からなるフィードバック制御ループと前記モータで駆動される減速機と前記減速機に接続する負荷からなる１軸分の関節軸とで構成するシステムにおける前記位置指令値から負荷速度までの伝達関数を基に負荷の位置、速度および加速度を算出する負荷状態量算出部と、前記負荷の位置、速度および加速度を入力して他軸からの干渉力を算出する逆動力学計算部と、前記干渉力をこれに見合った電流指令値に変換し前記電流制御部に入力する変換部を有するものである。
【００１１】
請求項２記載のロボット制御装置によれば、請求項１と同様な効果がある。
【００１４】
【発明の実施の形態】
次に本発明の一実施の形態を説明する。図１は、本発明の構成を示すブロック図である。従来技術の構成で説明したのと同じ要素については、同じ符号を付して説明を省略し、異なる部分のみ説明する。
【００１５】
従来技術の構成と異なるのは、指令値を入力し、マニピュレータの駆動に必要なトルクを設定値として、設定値に対するマニピュレータの姿勢変化に伴い、自軸のノミナルな慣性モーメントのトルクを除いた、動力学のトルク成分をトルク変動値とし、これを補償する補償トルク算出部１を付加した点である。
【００１６】
補償トルク算出部１は、負荷状態量算出部２、逆動力学計算部３、変換部４からなる。
【００１７】
負荷状態量算出部２は、位置指令を入力として、位置指令から負荷速度ωＬまでの伝達関数をもとに、負荷の位置、速度、加速度を算出する部分である。逆動力学計算部３は、負荷状態量算出部２からの、負荷の位置、速度、加速度を入力として、他軸からの干渉力τＬを算出する部分である。変換部４は、負荷状態量算出部２が算出した他軸からの干渉力τＬを、これに見合った電流指令値に変換する部分である。変換部４からの電流指令値は加算部５で速度制御部２２からの電流指令値に加算されて、電流制御部２３の入力となる。
【００１８】
以下、逆動力学計算部３について説明する。
【００１９】
ロボットマニピュレータの動力学方程式は、〔数１〕の式（１）のように表記される。式（１）の右辺第１項は角加速度によるトルクを表し、右辺第２項目は動摩擦によるトルクを表している。また、右辺第３項は、重力項・コリオリ項をまとめた項目である。これらの慣性モーメント及び重力・コリオリ項は、関節角度と関節角速度によって代数式で一意に決定できる。動摩擦に関しては、一定値であると見ることが出来る。
【００２０】
【数１】

【００２１】
これらの行列の計算を行って、動力学トルクを計算する。
【００２２】
ここで、各軸の制御系を考えると、ノミナルの慣性モーメントに対して自軸の角加速度の影響だけを考えて制御系を設計できるようにするため、他の軸の影響はすべて負荷トルク入力と見るようにする。
【００２３】
そのために、第１軸目（ＲＴ軸）で代表して示せば、負荷側の駆動トルクは〔数２〕の式（３）、（４）のように書くことが出来る。
【００２４】
【数２】

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

【００２７】
Ｈの対角項（慣性モーメント）から、各軸のノミナルの慣性モーメント値（当該軸の負荷の慣性モーメント値）を差し引いて、動力学によって変化した慣性モーメント（他軸の慣性モーメント）の成分も補償トルクとしてフィードバックする。これによって、ノミナル慣性分の負荷トルクを除外した負荷トルクを計算できるようにする。
【００２８】
なお、上記の式（５）で示した動力学計算には、負荷側の位置・速度・加速度が必要となるが、これは先に述べたように、負荷状態量算出部２で、関節位置の位置指令と位置指令から負荷速度までの伝達関数を用いて算出する。
【００２９】
上記式（５）にもとづいて逆動力学計算部３で算出された動力学トルクは、電流指令値の形で電流補償入力として与えて、負荷トルクを補償する。もちろん、電流でなくとも直接的にトルクとして与えることができるのであれば、それでも構わない。
【００３０】
電流指令値として補償する場合は、以下のようになる。
【００３１】
負荷トルクに対する〔数４〕の負荷速度の応答（式（７））と電流指令に対する負荷速度の応答（式（８））を求める。
【００３２】
【数４】

【００３３】
ここで、ωＬｔ、ωＬｉは負荷速度であり、τＬは負荷トルク、Ｉｃｍｐは電流指令値である。
【００３４】
式（７）、（８）について〔数５〕の式（９）のようにおくと、式（１１）が導かれる。
【００３５】
【数５】

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

【００３８】
つぎに、この発明のロボット制御方法は、上記ロボット制御装置を用いて、第１ステップは、ロボットのマニピュレータ（１５）を駆動するモータ１０の動作を指示する指令値を入力し、指令値に基づいて位置を制御する信号を出力する。第２ステップは、第１ステップで出力された信号に基づいてモータの回転速度を制御する信号を出力する。第３ステップは、第２ステップで出力された信号に基づいてモータ１０を駆動する電流を制御する。第４ステップは、第３ステップで出力された電流に基づいてモータ１０を駆動する。フィードバックステップは、第４ステップで駆動されたモータ１０の速度を検出し、その検出値を位置信号に変換して第１ステップにフィードバック入力し、上記検出値を第２ステップおよび第３ステップにフィードバック入力する。補償ステップは、指令値を入力し、マニピュレータの駆動に必要なトルクを設定値として、設定値に対するマニピュレータの姿勢変化に伴い、自軸のノミナルな慣性モーメントのトルクを除いた、動力学のトルク成分をトルク変動値とし、これを補償する補償トルクを算出し、補償ステップで算出される補償トルクに応じた出力信号を第３ステップに入力するものである。
【００３９】
【発明の効果】
請求項１記載のロボット制御方法によれば、マニピュレータの動きによらず、各関節軸の応答性を同じにして、マニピュレータ先端の軌跡誤差を少なくし、また、制御パラメタをマニピュレータの動きに関係なく設定できるようにして、制御系の設計を容易にすることができる。
【００４０】
請求項２および請求項３記載のロボット制御装置によれば、請求項１と同様な効果がある。
【図面の簡単な説明】
【図１】本発明の一実施の形態におけるロボット制御装置のブロック図である。
【図２】従来のロボット制御装置のブロック図である。
【符号の説明】
１補償トルク算出部
２負荷状態量算出部
３逆動力学計算部
４変換部
５加算部
１０モータ
１１、１２係数（減速比）
１３積分要素
１４係数（弾性係数）
１５負荷（アーム）
１６２慣性系
２０積分器
２１位置制御部
２２速度制御部
２３電流制御部
２４係数（トルク定数）
３０外乱トルク算出部
４２加算部
４３〜４５減算部

Claims

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