(57)【要約】  ロボットの関節を駆動するアクチュエータ２を制御するロボット制御装置が、記憶された振動周波数を出力する振動周波数記憶部（６）と、振動周波数を入力とし、振動信号を出力する振動信号生成部（７）と、位置指令に振動信号が加算された補正後位置指令を入力とし、アクチュエータ（２）の位置を制御する制御指令、関節の角度である関節角度及びアクチュエータ（２）の駆動トルクである実駆動トルクを出力する位置制御部（１）と、関節角度を入力とし、アクチュエータ（２）を駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部（３）と、駆動トルク推定値と実駆動トルクとを入力とし、駆動トルク推定値と実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部（４）と、第１の外乱トルク推定値を入力とし、第１の外乱トルク推定値から振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部（５）とを備える。
2016185600.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの関節を駆動するアクチュエータを制御するロボット制御装置であって、  記憶された振動周波数を出力する振動周波数記憶部と、  前記振動周波数を入力とし、振動信号を出力する振動信号生成部と、  位置指令に前記振動信号が加算された補正後位置指令を入力とし、前記アクチュエータの位置を制御する制御指令、前記関節の角度である関節角度及び前記アクチュエータの駆動トルクである実駆動トルクを出力する位置制御部と、  前記関節角度を入力とし、前記アクチュエータを駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部と、  前記駆動トルク推定値と前記実駆動トルクとを入力とし、前記駆動トルク推定値と前記実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部と、  前記第１の外乱トルク推定値を入力とし、前記第１の外乱トルク推定値から前記振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部とを備えることを特徴とするロボット制御装置。
【請求項２】
前記第２の外乱トルク推定値を入力とし、予め定められた慣性及び摩擦特性に基づくインピーダンス制御によって前記位置指令を生成して出力する位置指令生成部を備えることを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記振動信号生成部は、  前記関節角度を入力とし、関節角速度を出力する関節角速度演算部と、  前記関節角速度を入力とし、前記関節角速度の絶対値を出力する絶対値演算部と、  前記関節角速度の絶対値を入力とし、予め設定された関節角速度しきい値との比較により関節角速度比較信号を出力する関節角速度しきい値判定部と、  前記振動周波数及び前記関節角速度比較信号を入力とし、前記振動信号を出力する振動信号演算部とを備えることを特徴とする請求項１に記載のロボット制御装置。
【請求項４】
前記振動成分除去部は、  前記振動信号の振動周波数に対するノッチフィルタと、  前記振動信号の振動周波数の高調波に対するノッチフィルタとを備えることを特徴とする請求項１に記載のロボット制御装置。
【請求項５】
前記第２の外乱トルク推定値を入力とし、前記ロボットとは異なる関節を駆動するスレーブロボットのアクチュエータへのスレーブ制御指令を出力し、  前記スレーブロボットのアクチュエータの関節角度を前記位置指令として出力するスレーブトルク制御部を備えることを特徴とする請求項１に記載のロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、アクチュエータを用いてロボットを駆動するロボット制御装置に関する。
【背景技術】
【０００２】  作業者の誘導に従ってロボットを制御する直接教示、又はマスターとなるロボットに作用する外力をスレーブとなるロボットに伝送するマスタースレーブロボットにおいては、ロボットに作用する外力を推定し、推定した外力に基づいた制御を行う必要がある。そこで、ロボットの先端部に力覚センサを取り付けることが考えられる。ロボットの先端部に力覚センサを取り付けた場合には外力を高精度に測定できるものの、力覚センサ自体が高価であり、且つ衝撃に弱いという問題があるため、力覚センサの使用は敬遠されることが多い。そのため、力覚センサを使用することなく、アクチュエータから得られる位置及びトルクを用いて、ロボットに作用する外力を推定する方法が提案されている。【０００３】  このような技術の一例である特許文献１には、駆動軸静止時のトルク推定誤差を小さくすることを目的に、摩擦のヒステリシス特性の数学モデル、並びにトルク入力及び外乱トルク推定値の履歴を用いて摩擦のヒステリシス特性を補償する技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１０−２６４０５７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】  しかしながら、上記従来の技術である特許文献１に開示された技術によれば、摩擦のヒステリシス特性の数学モデルを使用しており、この数学モデルは最大静止摩擦力及びクーロン摩擦力をはじめとする摩擦パラメータを必要とする。そのため、実際の摩擦特性が数学モデルと異なる場合又は周辺温度の変化及びロボットの経年変化によって摩擦パラメータが変化した場合には、ロボットに作用する外乱トルクを高精度に推定することができない、という問題があった。【０００６】  本発明は、上記に鑑みてなされたものであって、ロボットに作用する外乱トルクを高精度に推定することが可能なロボット制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００７】  上述した課題を解決し、目的を達成するために、本発明は、ロボットの関節を駆動するアクチュエータを制御するロボット制御装置であって、記憶された振動周波数を出力する振動周波数記憶部と、前記振動周波数を入力とし、振動信号を出力する振動信号生成部と、位置指令に前記振動信号が加算された補正後位置指令を入力とし、前記アクチュエータの位置を制御する制御指令、前記関節の角度である関節角度及び前記アクチュエータの駆動トルクである実駆動トルクを出力する位置制御部と、前記関節角度を入力とし、前記アクチュエータを駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部と、前記駆動トルク推定値と前記実駆動トルクとを入力とし、前記駆動トルク推定値と前記実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部と、前記第１の外乱トルク推定値を入力とし、前記第１の外乱トルク推定値から前記振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部とを備えることを特徴とする。
【発明の効果】
【０００８】  本発明によれば、ロボットに作用する外乱トルクを高精度に推定することが可能なロボット制御装置を得ることができるという効果を奏する。
【図面の簡単な説明】
【０００９】
【図１】実施の形態１にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図
【図２】駆動トルク推定部の一構成例を示すブロック図
【図３】振動成分除去部の一構成例を示すブロック図
【図４】位置指令生成部の一構成例を示すブロック図
【図５】第１軸インピーダンスモデルの一構成例を示すブロック図
【図６】外乱トルクの推定値に基づいてインピーダンス制御を行うロボット制御装置の比較例を示すブロック図
【図７】実施の形態２にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図
【図８】振動信号生成部の一構成例を示すブロック図
【図９】実施の形態３にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図
【図１０】位置指令生成部の一構成例を示すブロック図
【図１１】実施の形態４にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図
【発明を実施するための形態】
【００１０】  以下に、本発明の実施の形態にかかるロボット制御装置を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１１】実施の形態１．  図１は、本発明の実施の形態１にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図である。図１に示すロボット制御装置は、ロボットの関節を駆動するアクチュエータ２を制御するロボット制御装置であって、記憶された振動周波数を出力する振動周波数記憶部６と、この振動周波数を入力とし、振動信号を出力する振動信号生成部７と、位置指令にこの振動信号が加算された補正後位置指令を入力とし、アクチュエータ２の位置を制御する制御指令、この関節の角度である関節角度及びアクチュエータ２の駆動トルクである実駆動トルクを出力する位置制御部１と、この関節角度を入力とし、アクチュエータ２を駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部３と、駆動トルク推定値と実駆動トルクとを入力とし、駆動トルク推定値と実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部４と、第１の外乱トルク推定値を入力とし、第１の外乱トルク推定値から振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部５とを備える。
【００１２】  位置制御部１は、補正後位置指令が入力されてロボットの各関節に設置されたアクチュエータ２の位置制御を行うための制御指令、ロボットの各関節の角度である関節角度及びアクチュエータ２の駆動トルクである実駆動トルクを出力する。
【００１３】  駆動トルク推定部３は、位置制御部１から出力された関節角度が入力されて、ロボットを駆動するのに必要な駆動トルクの推定を行い、駆動トルク推定値を算出して出力する。
【００１４】  図２は、駆動トルク推定部の一構成例を示すブロック図である。図２に示す駆動トルク推定部３は、重力トルク演算部３１と、関節角速度演算部３２と、摩擦トルク演算部３３と、加算部３４と、を備える。重力トルク演算部３１は、入力された関節角度からロボットの各関節の重力トルク推定値を算出して出力する。関節角速度演算部３２は、入力された関節角度を微分または差分演算することで、関節角速度を算出して出力する。摩擦トルク演算部３３は、関節角速度演算部３２から出力された関節角速度と、予め設定されたロボットのクーロン摩擦トルクと、粘性摩擦トルク係数と、により摩擦トルク推定値を算出して出力する。加算部３４は、重力トルク演算部３１から出力された重力トルク推定値と、摩擦トルク演算部３３から出力された摩擦トルク推定値とが入力されてこれらの加算を行うことで、駆動トルク推定値を算出して出力する。
【００１５】  なお、図２に示す駆動トルク推定部３は、重力トルク推定値と摩擦トルク推定値のみから駆動トルク推定値を算出しているが、本発明はこれに限定されるものではない。駆動トルク推定部３が、ロボットの運動方程式から定まる遠心コリオリトルク及び慣性トルクの推定値を駆動トルク推定値にさらに加えるように構成されていてもよい。
【００１６】  外乱トルク演算部４は、駆動トルク推定部３から出力された駆動トルク推定値と、位置制御部１から出力された実駆動トルクとの差を算出し、第１の外乱トルク推定値を出力する。
【００１７】  振動周波数記憶部６は、振動周波数を記憶し、出力する。
【００１８】  振動成分除去部５は、外乱トルク演算部４から出力された第１の外乱トルク推定値と、振動周波数記憶部６から出力された振動周波数とを入力とし、第１の外乱トルク推定値から振動周波数の周波数成分を除去し、第２の外乱トルク推定値を出力する。
【００１９】  図３は、振動成分除去部の一構成例を示すブロック図である。図３に示す振動成分除去部５は、振動周波数ノッチフィルタ５１と、３倍周波数ノッチフィルタ５２と、ローパスフィルタ５３とを備える。振動周波数ノッチフィルタ５１は、振動周波数の周波数成分を選択的に除去するフィルタである。３倍周波数ノッチフィルタ５２は、振動周波数の３倍の周波数成分を選択的に除去するフィルタである。ローパスフィルタ５３は、予め設定された遮断周波数以上の周波数成分を除去するフィルタである。図３に示す振動成分除去部５により、第１の外乱トルク推定値から振動周波数の周波数成分及びその高調波成分を効率的に除去することができる。すなわち、摩擦に起因したトルクの振動成分を効率的に除去することができる。
【００２０】  なお、図３に示す振動成分除去部５は、振動周波数のノッチフィルタと、その３倍の周波数のノッチフィルタとを備えるが、本発明はこれに限定されるものではない。振動成分除去部５は、振動周波数の５倍又は７倍の周波数成分を選択的に除去するフィルタを備えてもよいし、さらに高次の高調波成分を除去するノッチフィルタを備えてもよい。
【００２１】  図１に示す位置指令生成部９は、第２の外乱トルク推定値が入力されて、位置指令を算出して出力する。図４は、位置指令生成部の一構成例を示すブロック図である。図４に示す位置指令生成部９は、第１軸インピーダンスモデル９１と、第２軸インピーダンスモデル９２と、第３軸インピーダンスモデル９３と、第４軸インピーダンスモデル９４と、第５軸インピーダンスモデル９５と、第６軸インピーダンスモデル９６と、デマルチプレクサ９７と、マルチプレクサ９８とを備える。デマルチプレクサ９７は、第２の外乱トルク推定値を各軸の成分の信号に分解することで、第１軸から第６軸までの各軸の外乱トルク推定値を出力する。マルチプレクサ９８は、第１軸から第６軸までの各軸のインピーダンスモデルの出力を一つの信号にまとめて位置指令を出力する。
【００２２】  図５は、第１軸インピーダンスモデルの一構成例を示すブロック図である。なお、図５には第１軸インピーダンスモデル９１を示しているが、第２軸インピーダンスモデル９２、第３軸インピーダンスモデル９３、第４軸インピーダンスモデル９４、第５軸インピーダンスモデル９５及び第６軸インピーダンスモデル９６の構成も同様である。
【００２３】  図５に示す第１軸インピーダンスモデル９１は、不感帯９０１と、減算部９０２と、モデル慣性記憶部９０３と、除算部９０４と、速度積分器９０５と、摩擦力演算部９０６と、位置積分器９０７とを備える。第１軸インピーダンスモデル９１は、不感帯９０１と、モデル慣性記憶部９０３と、摩擦力演算部９０６と、によって定まるインピーダンス特性に従って、第１軸外乱トルク推定値に対する速度指令を算出し、位置積分器９０７は、この速度指令を入力とし、第１軸位置指令を算出して出力する。なお、速度積分器９０５及び位置積分器９０７の積分値の範囲に上限及び下限を設定すると、インピーダンスモデルの位置及び速度の範囲制限を行うことも可能である。
【００２４】  図５に示すインピーダンスモデルを備える位置指令生成部９により、第２の外乱トルク推定値によるインピーダンス制御を行うことができる。このように、高精度に推定された第２の外乱トルク推定値を入力とし、予め定められた慣性及び摩擦特性に基づくインピーダンス制御を行うことで、ロボットの摩擦トルクによらずに良好なインピーダンス特性を実現することができる。なお、ここでインピーダンス制御は、ロボットに外力を加えた場合に生じる機械的なインピーダンスを、目的に沿う値に設定して制御する手法である。
【００２５】  振動信号生成部７は、振動周波数記憶部６から出力された振動周波数の正弦波から、ロボットの各関節に対して算出した信号を振動信号として出力する。この振動信号の振幅は、ロボットの各関節の振動によるロボットの手先位置の振動の振幅が十分に小さくように、一例として、ロボットの手先位置の振動の振幅が０．１ｍｍ以内となるように、予め設定されている。また、この振動信号の振幅は、ロボットの各関節に対して異なる値を設定することも可能である。
【００２６】  加算部８は、位置指令生成部９から出力された位置指令と、振動信号生成部７から出力された振動信号とを加算して補正後位置指令を出力する。
【００２７】  次に、実施の形態１のロボット制御装置の動作について説明する。図６は、外乱トルクの推定値に基づいてインピーダンス制御を行うロボット制御装置の比較例を示すブロック図である。図６において、図１に示すロボット制御装置と同等の機能を有する構成要素には同一の符号を付してその説明を省略する。
【００２８】  図６に示すロボット制御装置は、図１に示すロボット制御装置から、振動成分除去部５と、振動周波数記憶部６と、振動信号生成部７と、加算部８と、を除いた構成である。図６に示すロボット制御装置においては、外乱トルク演算部４が駆動トルク推定値と実駆動トルクとの差を算出して外乱トルク推定値を出力し、この外乱トルク推定値から位置指令生成部９で位置指令を算出することで、インピーダンス制御を実現することができる。
【００２９】  しかし、ロボットが静止している場合には、静止摩擦トルクの大きさが不定となるため、静止摩擦トルクとロボットに作用する外乱トルクとを分離して推定することができない。そのため、図６に示すロボット制御装置では、ロボットが静止している状態において、ロボットに作用する外乱トルクを正確に推定することができず、良好なインピーダンス特性を得ることができないという問題がある。
【００３０】  一方、図１に示すロボット制御装置では、振動信号生成部７で生成した振動信号を位置指令に加えた補正後位置指令によってアクチュエータ２の位置制御を行うことで、ロボットの各関節に微小な振動を発生させるので、ロボットの各関節が振動信号の振動周波数で駆動されて、振動信号の振動周波数と同一周波数の摩擦トルクがロボットの各関節に発生する。さらには、図１に示すロボット制御装置では、振動成分除去部５により、第１の外乱トルク推定値から振動信号の周波数の振動成分を除去するので、第１の外乱トルク推定値から摩擦トルクに起因したトルクの振動成分を除去することができ、振動成分除去部５から出力された第２の外乱トルク推定値は摩擦トルクが除去された外乱トルク推定値である。従って、この第２の外乱トルク推定値を外乱トルク推定値とすることで、摩擦トルクの影響が除去された外乱トルク推定値を得ることができる。
【００３１】  このように、本実施の形態にて説明したロボット制御装置は、振動信号生成部７と、振動成分除去部５とを組み合わせることにより、外乱トルク推定値から静止摩擦トルクの影響を除去することができる。そのため、摩擦トルクよりも小さい大きさの外乱トルクを高精度に推定することができる。また、高精度な外乱トルク推定値によってインピーダンス制御を行うことにより、ロボットの摩擦に影響されずに良好なインピーダンス特性を得ることができる。従って、本実施の形態にて説明したロボット制御装置は、直接教示のようにロボットに作用する力に応じてロボットを制御する用途において有用である。
【００３２】  なお、本実施の形態のロボット制御装置は、６軸のロボットを想定しているが本発明はこれに限定されるものではない。位置指令生成部９の軸数を変更することにより、水平多関節ロボットのような軸数の異なるロボットにも適用することが可能である。
【００３３】実施の形態２．  図７は、本発明の実施の形態２にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図である。図７において、図１に示すロボット制御装置と同等の機能を有する構成要素には同一の符号を付してその説明を省略する。図７に示すロボット制御装置は、振動信号生成部７に代えて振動信号生成部７ａを備える点が図１に示すロボット制御装置と異なる。
【００３４】  振動信号生成部７ａは、振動周波数記憶部６から出力された振動周波数と、位置制御部１から出力された関節角度とを入力とし、振動信号を生成して出力する。振動信号生成部７ａは、振動周波数のみならず関節角度も入力されている点が図１における振動信号生成部７と異なる。
【００３５】  図８は、振動信号生成部の一構成例を示すブロック図である。図８に示す振動信号生成部７ａは、関節角速度演算部７１と、絶対値演算部７２と、関節角速度しきい値判定部７３と、振動信号演算部７４とを備える。関節角速度演算部７１は、入力された関節角度を微分または差分演算して関節角速度を算出する。絶対値演算部７２は、関節角速度演算部７１から出力された関節角速度の絶対値を算出して出力する。関節角速度しきい値判定部７３は、絶対値演算部７２から出力された関節角速度の絶対値を入力とし、関節角速度の絶対値の各軸の成分に対して予め設定された関節角速度しきい値との比較を行うことで、関節角速度比較信号を出力する。この際、関節角速度の絶対値が関節角速度しきい値以下である場合には真値を出力し、関節角速度の絶対値が関節角速度しきい値より大きい場合には偽値を出力する。この関節角速度しきい値は、関節角速度の絶対値が関節角速度しきい値以下である場合には、ロボットが静止しているとみなせる値に予め設定されている。また、この関節角速度しきい値は、ロボットの各関節に対して異なる値を設定することも可能である。
【００３６】  振動信号演算部７４は、振動周波数と、関節角速度しきい値判定部７３から出力された関節角速度比較信号と、予め設定された振動振幅とにより、振動信号を出力する。この際、関節角速度比較信号の該当軸の成分が真値である場合には該当軸の振動信号に振動周波数の正弦波信号を出力し、偽値である場合には該当軸の振動信号に０を出力する。この振動信号の振幅は、ロボットの各関節の振動によるロボットの手先位置の振動の振幅が十分に小さくなるように、一例として、ロボットの手先位置の振動の振幅が０．１ｍｍ以内となるように、予め設定されている。また、この振動信号の振幅は、ロボットの各関節に対して異なる値を設定することも可能である。  
【００３７】  図８に示す振動信号生成部７ａにより、ロボットの該当軸の関節角速度の絶対値が関節角速度しきい値以下であり、ロボットの該当軸が静止しているとみなせる場合には、該当軸の振動信号には正弦波信号が出力される。一方、ロボットの該当軸が静止していない場合には、該当軸の振動信号には０が出力される。
【００３８】  次に、実施の形態２のロボット制御装置の動作について説明する。振動信号生成部７ａは、ロボットが静止している場合には、振動信号として正弦波信号を出力し、ロボットが静止していない場合には、振動信号として０を出力する。
【００３９】  ロボットが静止している場合には、振動信号生成部７ａから出力された正弦波信号を位置指令に加えた補正後位置指令によってロボットの位置制御が行われるため、振動信号の振動周波数と同一周波数の摩擦トルクがロボットの各関節に発生する。さらには、振動成分除去部５によって第１の外乱トルク推定値に含まれる振動周波数成分が除去される。従って、実施の形態１のロボット制御装置と同様に、摩擦トルクの影響を除去することで高精度な外乱トルク推定値を得ることができる。
【００４０】  一方、ロボットが静止していない場合には、ロボットの関節に発生する摩擦トルクは関節角速度に応じて一意に定まるので、摩擦トルク演算部３３から出力された摩擦トルク推定値を用いて摩擦トルクの補償を行うことによって、第１の外乱トルク推定値から摩擦トルクの影響を除去することができる。従って、ロボットが静止していない場合には、位置指令に振動信号を加えなくても高精度な外乱トルク推定値の算出が可能である。
【００４１】  以上説明したように、本実施の形態のロボット制御装置は、ロボットに作用する外乱トルクから摩擦トルクの影響を除去することで外乱トルクを高精度に推定することができる。また、高精度な外乱トルク推定値によってインピーダンス制御を行うことにより、ロボットの摩擦に影響されずに良好なインピーダンス特性を得ることができる。従って、本実施の形態にて説明したロボット制御装置は、直接教示のようにロボットに作用する力に応じてロボットを制御する用途において有用である。
【００４２】  なお、本実施の形態のロボット制御装置は、ロボットが静止していない場合には振動信号を０とすることによりロボットの振動を停止させるため、実施の形態１のロボット制御装置と比較して、位置制御の精度を改善することが可能である。
【００４３】実施の形態３．  図９は、本発明の実施の形態３にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図である。図９において、図１，７に示すロボット制御装置と同等の機能を有する構成要素には同一の符号を付してその説明を省略する。図９に示すロボット制御装置は、位置指令生成部９に代えて位置指令生成部９ａを備える点が図７に示すロボット制御装置と異なる。図９に示す位置指令生成部９ａは、第２の外乱トルク推定値及び関節角度を入力とし、位置指令を出力する。
【００４４】  図１０は、位置指令生成部の一構成例を示すブロック図である。図１０に示す位置指令生成部９ａは、Ｘ軸並進インピーダンスモデル９１１と、Ｙ軸並進インピーダンスモデル９１２と、Ｚ軸並進インピーダンスモデル９１３と、Ｘ軸回転インピーダンスモデル９１４と、Ｙ軸回転インピーダンスモデル９１５と、Ｚ軸回転インピーダンスモデル９１６と、デマルチプレクサ９１７と、マルチプレクサ９１８と、ヤコビ行列演算部９１９と、外乱力演算部９２０と、逆運動学演算部９２１とを備える。ヤコビ行列演算部９１９は、関節角度を入力とし、ロボットの関節座標の微小変位からロボットの手先座標の微小変位への変換行列であるヤコビ行列を算出して出力する。外乱力演算部９２０は、第２の外乱トルク推定値を入力とし、ヤコビ行列演算部９１９から出力されたヤコビ行列の転置逆行列を第２の外乱トルク推定値にかけることで、関節座標系で表された外乱トルクを手先座標系で表された外乱力に変換する。
【００４５】  Ｘ軸並進インピーダンスモデル９１１からＺ軸回転インピーダンスモデル９１６までの各軸のインピーダンスモデルは、図５に示す第１軸インピーダンスモデル９１と同様である。これらのインピーダンスモデルは、外乱力演算部９２０から出力された外乱力の各成分に対して、インピーダンスモデルで設定されたインピーダンス特性によって位置指令を算出する。デマルチプレクサ９１７は、外乱力演算部９２０から出力された外乱力を各軸の成分に分解することで、Ｘ，Ｙ，Ｚの各軸の並進力とトルクを出力する。マルチプレクサ９１８は、各軸のインピーダンスモデルの出力を一つの信号にまとめて手先座標位置指令を出力する。逆運動学演算部９２１は、マルチプレクサ９１８から出力された手先座標位置指令を入力とし、逆運動学演算と呼ばれるロボットの手先座標系から関節座標系への変換演算を行い、手先座標位置指令を関節座標に変換した位置指令を出力する。
【００４６】  次に、実施の形態３のロボット制御装置の動作について説明する。図９に示すロボット制御装置は、位置指令生成部９ａ以外の構成要素が図７に示すロボット制御装置と同一である。そのため、図７に示すロボット制御装置と同様に、ロボットに作用する外乱トルクを高精度に推定することができる。また、高精度な外乱トルク推定値によってインピーダンス制御を行うことにより、良好なインピーダンス特性を持つインピーダンス制御を実現することが可能である。
【００４７】  さらには、図９に示すロボット制御装置は、位置指令生成部９ａにおいて手先座標系によるインピーダンス制御演算を行っている。そのため、本実施の形態のロボット制御装置を直接教示に適用した場合には、ロボットの各関節の動作を連動させた直観的な教示操作を容易に行うことが可能である。
【００４８】実施の形態４．  図１１は、本発明の実施の形態４にかかるロボット制御装置の一構成例と、このロボット制御装置からの制御指令が入力されるアクチュエータとを示すブロック図である。図１１において、図１，７に示すロボット制御装置と同等の機能を有する構成要素には同一の符号を付してその説明を省略する。図１１に示すロボット制御装置には、スレーブトルク制御部１０が追加され、スレーブロボットが追加されたマスタースレーブ構成としている。
【００４９】  スレーブトルク制御部１０は、振動成分除去部５から出力された第２の外乱トルク推定値をトルク指令として、スレーブロボットの関節に設置されたスレーブロボットのアクチュエータ１１のトルク制御を行うためのスレーブ制御指令を出力する。スレーブトルク制御部１０は、スレーブロボットのアクチュエータ１１の位置を検出して、検出信号を位置指令として出力する。
【００５０】  なお、振動信号生成部７ａは、振動周波数記憶部６から出力された振動周波数と、位置制御部１から出力された関節角度とを入力とし、関節角度から計算した関節角速度の絶対値と予め設定された関節角速度しきい値との比較結果に応じて正弦波信号と０信号とを切り替えることで、振動信号を出力する。
【００５１】  次に、実施の形態４のロボット制御装置の動作について説明する。図１１に示すロボット制御装置は、振動信号生成部７ａと振動成分除去部５とを組み合わせることにより、図７に示すロボット制御装置と同様に、外乱トルクから摩擦トルクの影響を除去することで外乱トルクを高精度に推定することができる。また、図１１に示すロボット制御装置は、外乱トルク推定値をスレーブロボットに伝達し、スレーブロボットのトルクから位置までの実際の動特性に従ってマスターロボットの位置指令を生成する。このような構成とすることにより、スレーブロボットの動特性をマスターロボット上に再現することができるため、マスターロボットの操作者は、スレーブロボットの動特性及びスレーブロボットに加わる外力を把握しつつ、スレーブロボットの遠隔操作を行うことが可能である。
【００５２】  また、本実施の形態のロボット制御装置は、外乱トルクから摩擦トルクの影響を除去することで、摩擦トルク以下の大きさの小さい外乱トルクを高精度に推定することができるため、良好な操作感覚の遠隔操作を実現することができる。
【００５３】  なお、マスターロボットに作用する外乱トルクをスレーブロボットに伝送し、スレーブロボットの位置をマスターロボットの位置に伝送するバイラテラル制御によれば、マスターロボットの操作者がスレーブロボットを遠隔操作することができ、また、スレーブロボットに加わる反力を操作者に提示することができる制御として知られている。以上説明したように、本実施の形態のロボット制御装置は、力覚センサに代表されるセンサを使用せずとも高精度に外乱トルクを推定することができ、センサを使用しない安価なバイラテラル制御系の構成であり、マスタースレーブロボットを実現するに際して有用である。
【００５４】  以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。
【符号の説明】
【００５５】  １  位置制御部、２  アクチュエータ、３  駆動トルク推定部、４  外乱トルク演算部、５  振動成分除去部、６  振動周波数記憶部、７，７ａ  振動信号生成部、８  加算部、９，９ａ  位置指令生成部、１０  スレーブトルク制御部、１１  スレーブロボットのアクチュエータ、３１  重力トルク演算部、３２  関節角速度演算部、３３  摩擦トルク演算部、３４  加算部、５１  振動周波数ノッチフィルタ、５２  ３倍周波数ノッチフィルタ、５３  ローパスフィルタ、７１  関節角速度演算部、７２  絶対値演算部、７３  関節角速度しきい値判定部、７４  振動信号演算部、９１  第１軸インピーダンスモデル、９２  第２軸インピーダンスモデル、９３  第３軸インピーダンスモデル、９４  第４軸インピーダンスモデル、９５  第５軸インピーダンスモデル、９６  第６軸インピーダンスモデル、９７  デマルチプレクサ、９８  マルチプレクサ、９０１  不感帯、９０２  減算部、９０３  モデル慣性記憶部、９０４  除算部、９０５  速度積分器、９０６  摩擦力演算部、９０７  位置積分器、９１１  Ｘ軸並進インピーダンスモデル、９１２  Ｙ軸並進インピーダンスモデル、９１３  Ｚ軸並進インピーダンスモデル、９１４  Ｘ軸回転インピーダンスモデル、９１５  Ｙ軸回転インピーダンスモデル、９１６  Ｚ軸回転インピーダンスモデル、９１７  デマルチプレクサ、９１８  マルチプレクサ、９１９  ヤコビ行列演算部、９２０  外乱力演算部、９２１  逆運動学演算部。
【図１】
2016185600.tif 000003
【図２】
2016185600.tif 000004
【図３】
2016185600.tif 000005
【図４】
2016185600.tif 000006
【図５】
2016185600.tif 000007
【図６】
2016185600.tif 000008
【図７】
2016185600.tif 000009
【図８】
2016185600.tif 000010
【図９】
2016185600.tif 000011
【図１０】
2016185600.tif 000012
【図１１】
2016185600.tif 000013
【手続補正書】
【提出日】平成29年5月18日(2017.5.18)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
ロボットの関節を駆動するアクチュエータを制御するロボット制御装置であって、
記憶された振動周波数を出力する振動周波数記憶部と、
前記関節の角度である関節角度及び前記振動周波数を入力とし、前記関節角度から算出した関節角速度の絶対値と予め設定された関節角速度しきい値との比較に基づき振動信号を出力する振動信号生成部と、
位置指令に前記振動信号を加算した補正後位置指令を出力する加算部と、
前記補正後位置指令を入力とし、前記アクチュエータの位置を制御する制御指令、前記関節角度及び前記アクチュエータの駆動トルクである実駆動トルクを出力する位置制御部と、
前記関節角度を入力とし、前記アクチュエータを駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部と、
前記駆動トルク推定値と前記実駆動トルクとを入力とし、前記駆動トルク推定値と前記実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部と、
前記第１の外乱トルク推定値を入力とし、前記第１の外乱トルク推定値から前記振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部とを備えることを特徴とするロボット制御装置。
【請求項２】
前記第２の外乱トルク推定値を入力とし、予め定められた慣性及び摩擦特性に基づくインピーダンス制御によって前記位置指令を生成して出力する位置指令生成部を備えることを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記振動成分除去部は、
前記振動信号の振動周波数に対するノッチフィルタと、
前記振動信号の振動周波数の高調波に対するノッチフィルタとを備えることを特徴とする請求項１に記載のロボット制御装置。
【請求項４】
前記第２の外乱トルク推定値を入力とし、前記ロボットとは異なる関節を駆動するスレーブロボットのアクチュエータへのスレーブ制御指令を出力し、
前記スレーブロボットのアクチュエータの関節角度を前記位置指令として出力するスレーブトルク制御部を備えることを特徴とする請求項１に記載のロボット制御装置。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】０００７
【補正方法】変更
【補正の内容】
【０００７】
上述した課題を解決し、目的を達成するために、本発明は、ロボットの関節を駆動するアクチュエータを制御するロボット制御装置であって、記憶された振動周波数を出力する振動周波数記憶部と、前記関節の角度である関節角度及び前記振動周波数を入力とし、前記関節角度から算出した関節角速度の絶対値と予め設定された関節角速度しきい値との比較に基づき振動信号を出力する振動信号生成部と、位置指令に前記振動信号を加算した補正後位置指令を出力する加算部と、前記補正後位置指令を入力とし、前記アクチュエータの位置を制御する制御指令、前記関節角度及び前記アクチュエータの駆動トルクである実駆動トルクを出力する位置制御部と、前記関節角度を入力とし、前記アクチュエータを駆動するのに必要な駆動トルク推定値を出力する駆動トルク推定部と、前記駆動トルク推定値と前記実駆動トルクとを入力とし、前記駆動トルク推定値と前記実駆動トルクとの差を第１の外乱トルク推定値として出力する外乱トルク演算部と、前記第１の外乱トルク推定値を入力とし、前記第１の外乱トルク推定値から前記振動周波数の周波数成分が除去された第２の外乱トルク推定値を出力する振動成分除去部とを備えることを特徴とする。
【国際調査報告】
2016185600.tif 000014
2016185600.tif 000015
2016185600.tif 000016
2016185600.tif 000017