【０００１】
【発明の属する技術分野】
本発明は、インピーダンス制御を実現するロボットの制御装置に関し、特にロボットに作用する外部環境からの力に対してリアルタイムに外部環境の剛性係数を推定し、ロボットのアームを駆動制御するようにしたインピーダンス制御装置などに関する。
【０００２】
【従来の技術】
ロボットの動きを制御する方法の一つとしてインピーダンス制御なる方法が知られている。このインピーダンス制御は、ロボットのエンドエフェクタに発生する外力によってロボットに見かけ上の柔らかさを実現するものであり、その外力をＦ、エンドエフェクタの位置をｐとした場合に、下記の式を用いて実現される。
【０００３】
Ｍｐ”＋Ｄｐ’＋Ｋｐ＝Ｆ …式（ａ）
【０００４】
なお、ここでＭは慣性係数、Ｄは粘性係数、Ｋは剛性係数を示すものであり、ロボットの各部を制御する際にあらかじめ定められるものである。また、ｐ”は加速度、ｐ’は速度を示すものである。
【０００５】
一方、このロボットのエンドエフェクタに作用する外部環境の力Ｆは、簡素化した式ではフックの法則に従い、
【０００６】
Ｆ＝Ｋｈ（ｐ−ｐ０） …式（ｂ）
【０００７】
で表される。ここで、Ｋｈはエンドエフェクタに作用する外部環境の剛性係数、ｐはエンドエフェクタに接触する人間の手先などの外部環境の位置（すなわち、エンドエフェクタの位置）、ｐ０はその外部環境の外力がゼロとなる平衡点を示すものである。そして、式（ａ）および式（ｂ）より、
【０００８】
Ｍｐ”＋Ｄｐ’＋Ｋｐ＝Ｋｈ（ｐ−ｐ０） …式（ｃ）
【０００９】
が成り立ち、この式（ｃ）からエンドエフェクタの位置ｐを算出して、その位置にエンドエフェクタを移動させるようにロボットの各部を制御する。
【００１０】
ところで、例えば、ロボットと人間とが協働して重量物を運搬するような場合、外部環境である人間の外力とその力の平衡点は常に変化する。このため、式（ｃ）だけでは、未知数が３つ（エンドエフェクタの位置ｐ、平衡点の位置ｐ０ 、外部環境の弾性係数Ｋｈ）存在するためエンドエフェクタの位置ｐを算出することができない。
【００１１】
このため、従来では、人間がエンドエフェクタを把持し、その手先位置を一定の位置に固定した状態でロボットを動かして手先のインピーダンス特性（Ｋｈ）の計測を行う方法や（「姿勢維持中の人間の手先インピーダンス推定」（計測自動制御学会論文集、３０−３、１９９４、Ｐ３１９―３２８）、同様に人間がエンドエフェクタを把持し、人間とエンドエフェクタが等速度運動を行っている状態で、その手先のインピーダンス特性（Ｋｈ）を計測して式（ｃ）のｐを算出できるようにしている（「水平面における多間接運動中の人腕機会インピーダンスの計測」（計測自動制御学会論文集２３−３、１９９６、Ｐ３６９―３７８）、「等尺性筋収縮における人間の手先インピーダンスの解析」（計測自動制御学会論文集３２−２、１９９６、Ｐ２７１―２８０）。
【００１２】
【発明が解決しようとする課題】
しかしながら、これらの文献に記載された方法では、人間の手先を静止もしくは等速度運動させた状態でロボットを強制的に動かす必要があり、ロボットと人間とがリアルタイムで協働作業する場合は、そのロボットの強制的な動きが人間の作業の邪魔となる。
【００１３】
また、これらの文献の方法では、外部環境の力Ｆを
【００１４】
Ｆ＝Ｍｈｐ”＋Ｄｈｐ’＋Ｋｈｐ
【００１５】
と詳細に表し、この外部環境側の慣性係数Ｍｈ、粘性係数Ｄｈ、剛性係数Ｋｈを求める必要があるために位置の情報を２回微分しているが、多くの自由度を有するロボットを用いて位置情報を得る場合、自由度が多ければ多いほど、ノイズの影響が大きくなる。これは、ロボットのエンドエフェクタを求めるために、ロボットが有するすべての関節角度の情報を利用しなければならないからである。一般に、ノイズは微分を行う毎に増加するため、これらの文献の方法では、多自由度を有するロボットを使用した場合には適用できない。
【００１６】
更に、これらの文献の方法では、外力と位置の計測を行った後にエンドエフェクタのインピーダンス特性を推定し、特に、計測後に強制的に加えた摂動部分のデータだけを取り出してＫｈの推定を行うようにしているため、人間とロボットとの協働作業中にリアルタイムでＫｈ推定を行うことができない。
【００１７】
そこで、本発明は上記課題に着目してなされたもので、ロボットが強制的に人間の手先を動かすことなく、リアルタイムにその外部環境の剛性係数（Ｋｈ）を推定し、高精度にロボットの各部を制御できるようにしたインピーダンス制御装置を提供することを目的とする。
【００１８】
【課題を解決するための手段】
本発明は、上記課題を解決するために、エンドエフェクタに作用する外部環境の力とその位置からアクチュエータを駆動制御する際、まず、エンドエフェクタに作用する外部環境の力変動の時間差分と、その外部環境の作用位置変動の時間差分を計測し、これらの差分値を用いて外部環境の剛性係数を算出する。そして、予め設定されたロボット側のインピーダンス特性である慣性係数および粘性係数および剛性係数と、前記算出された外部環境の剛性係数を用いてエンドエフェクタの移動位置を算出し、そのエンドエフェクタの位置に関する情報を出力してロボットのアクチュエータを駆動制御するようにしたものである。
【００１９】
このように構成すれば、例えば、差分時間を短く設定することによって、外部環境の平衡点ｐ０を無視することができ、式（ａ）を
【００２０】
ΔＦｅｓｔ（ｔ）＝ＫｈΔｐｅｓｔ（ｔ）
【００２１】
と近似して平衡点に関する項ｐ０を無視することができる。そして、このＦ、ｐを差分時間毎にセンシングすることによってＫｈを求め、また、このＫｈと、あらかじめ設定されたロボット側の慣性係数Ｍ、粘性係数Ｄ、剛性係数Ｋを用いてエンドエフェクタの位置ｐを求めることができるようになる。
【００２２】
また、エンドエフェクタの位置変動の時間差分を計測する場合、エンドエフェクタの移動速度を考慮して位置差分値を計測する。
【００２３】
通常、エンドエフェクタが高速度域で移動している場合、外部環境の平衡点もこれに伴って高速移動するため、平衡点の項ｐ０を無視することができなくなる。しかし、位置変動の時間差分計測の際にエンドエフェクタの移動速度を考慮した項を含めて演算するようにすれば、高速度域における平衡点の移動の問題を解決することができるようになる。
【００２４】
更に、エンドエフェクタの位置変動の時間差分を計測する場合、ロボットのコンプライアンスを考慮して位置差分値を計測する。
【００２５】
このように構成すれば、ロボットが有するコンプライアンスが原因となる位置誤差を補正することができ、より正確な位置変動の時間差分を計測することができるようになる。
【００２６】
加えて、外部環境の剛性係数を算出する際、計測した力差分値、位置差分値をフィルタにかけ、ノイズを除去した後にその剛性係数を算出する。
【００２７】
通常、ロボットの自由度が大きくなればなるほどセンサに作用するノイズが増えるが、このように構成すれば、ノイズをフィルタにより除去するため、より正確な剛性係数の算出が可能になる。
【００２８】
また、このような発明において、算出された外部環境の剛性係数と相関関係をもってロボット側のインピーダンス特性を変化させ、これらの値を用いてエンドエフェクタの位置を算出する。
【００２９】
このように構成すれば、外部環境から作用する力の大きさに対応してロボット側の動きの硬さやしなやかさを変化させることができ、より人間との協働作業に適した制御を行うことができるようになる。
【００３０】
そして、好ましくは、この相関関係を、外部環境の剛性係数の増大に伴って粘性係数を大きくするようなものにする。
【００３１】
このように構成すれば、例えば、人間とロボットとが協働して重量物を運搬するような場合、人間側の剛性係数（Ｋｈ）の増大に伴ってロボット側の動きを硬いものにすることができ、重量物をしっかりと運搬することができる。また、逆に、軽量物を迅速に運搬するような場合は、その人間側の剛性係数（Ｋｈ）の減少に伴ってロボット側の動きをしなやかにすることができ、より迅速な協働作業を実現することができるようになる。
【００３２】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照して説明する。図１は、本実施の形態におけるロボット１と、インピーダンス制御装置２のブロック図を示したものであり、図２はインピーダンス制御のブロック線図を示したものである。このインピーダンス制御装置２は、パーソナルコンピュータ２ａおよびサーボドライバ２ｂなどによって構成され、このインピーダンス制御装置２およびロボット１を接続して全体としてロボットシステム１００を構成している。
【００３３】
このロボット１は、周知のように、先端部にエンドエフェクタ１１（図２参照）を有するアーム１０およびこのアーム１０を駆動するアクチュエータであるモータ１３ａなどで構成される駆動手段１３と、エンドエフェクタ１１に作用する外部環境の力を計測する力覚センサ１２と、モータ１３ａの回転角度を検出するためのエンコーダ１４などを具備して構成される。
【００３４】
一方、インピーダンス制御装置２は、力差分計測手段２０と、位置差分計測手段２１と、剛性係数算出手段２２と、位置算出手段２３と、制御手段２４とを具備して構成される。
【００３５】
力差分計測手段２０は、エンドエフェクタ１１近傍に設けられた力覚センサ１２から得られる外力の値を微少時間毎にサンプリングし、次に示す式（１）で表現される力差分値ΔＦｅｓｔ（ｔ）を算出する。
【００３６】
ΔＦｅｓｔ（ｔ）＝ＦＲ（ｔ）−ＦＲ（ｔ−ｔｓ） …式（１）
【００３７】
このサンプリング時間は、外部環境の平衡点の移動を無視できる程度に設定するのが好ましく、例えば、サンプリング時間ｔｓ＝０．１５ｓｅｃなどに設定する。
【００３８】
位置差分計測手段２１は、エンコーダ１４から得られる回転角、もしくは、エンドエフェクタ１１の位置を検出しうる位置検出センサなどが設けられている場合はその位置センサから得られる位置情報を微少時間毎にサンプリングし、式（２）で表わされる位置差分値Δｐｅｓｔ（ｔ）を算出する。
【００３９】
Δｐｅｓｔ（ｔ）＝ｐＲ（ｔ）―ｐＲ（ｔ−ｔｓ） …式（２）
【００４０】
このサンプリング時間は、力差分計測手段で計測するサンプリング時間と同じ時間に設定するのが好ましく、ｔｓ＝０．１５ｓｅｃなどに設定する。
【００４１】
なお、外部環境の位置の変動が高速度域における場合には、サンプリング時間における平衡点の移動を無視することができず、速度によって生じる平衡点の位置の変動補正を考慮する必要が生じる。このため、ロボット１のエンドエフェクタ１１の位置補正を行う。まず、ロボット１のエンドエフェクタ１１の速度をｖＲ（ｔ）とすると、時間差分ｔｓの間に移動するエンドエフェクタ１１の距離は、ｔｓｖＲ（ｔ−ｔｓ）となる。そこで、式（２）について速度変動に対する補正を行い、
【００４２】
Δｐｅｓｔ（ｔ）＝ｐＲ（ｔ）―ｐＲ（ｔ−ｔｓ）―ｔｓｖＲ（ｔ−ｔｓ） …式（３）
【００４３】
とすることで、平衡点が移動する場合における問題の解決を行う。
【００４４】
さらに、ロボット１のエンドエフェクタ１１の位置（ｐＲ）には、ロボット１のコンプライアンスが原因となってエンドエフェクタ１１の位置に誤差を生ずる。このためエンドエフェクタ１１の位置誤差をリアルタイムに求め、これによってロボット１のエンドエフェクタ１１の位置補正も行う。コンプライアンスから生じる位置誤差は、
【００４５】
Δｐｃ（ｔ）＝Ｃｅｎｄ（ｔ）ＦＲ（ｔ） …式（４）
【００４６】
によって求められる。ここで、Δｐｃはロボット１のコンプライアンスによって生じる位置誤差を示し、また、Ｃｅｎｄ（ｔ）は、ロボット１のエンドエフェクタ１１におけるコンプライアンス行列を示す。この式（４）で求められる位置誤差を用いてロボット１のエンドエフェクタ１１の位置補正を行い、補正を行った後の位置をｐ＊Ｒ（ｔ）とすると、
【００４７】
ｐ＊Ｒ（ｔ）＝ｐＲ（ｔ）＋Δｐｃ（ｔ） …式（５）
【００４８】
となる。そして、式（３）のｐＲについてもコンプライアンスの位置誤差を考慮してｐ＊Ｒに置換すると、式（３）は、
【００４９】
Δｐｅｓｔ（ｔ）＝ｐ＊Ｒ（ｔ）―ｐ＊Ｒ（ｔ−ｔｓ）―ｔｓｖＲ（ｔ−ｔｓ） …式（６）
【００５０】
となる。そして、このような速度変動およびコンプライアンスを考慮して求められた式（６）のΔｐｅｓｔ（ｔ）を剛性係数算出手段２２に渡して剛性計数Ｋｈを推定する。
【００５１】
剛性係数算出手段２２は、このようにして計測された力差分値ΔＦｅｓｔ（ｔ）（式（１））および位置差分値Δｐｅｓｔ（ｔ）（式（６））をもとに外部環境である人間の手先の剛性係数（Ｋｈ）を算出する。一般に、外部環境の平衡点（外力がゼロとなる位置）をｐ０とした場合、フックの法則に従い、外部環境の力ＦＲとｐＲとは次式の関係を有する。
【００５２】
ＦＲ（ｔ）＝Ｋｈ（ｐＲ（ｔ）―ｐ０（ｔ）） …式（７）
【００５３】
ところで、外部環境の位置の変動が低速度域における場合は、サンプリング時間内においては、平衡点の移動は微少範囲にとどまると仮定でき（ｐ０（ｔ）≒ｐ０（ｔ−ｔｓ））、隣接サンプリング時間における平衡点の移動を無視することができる。よって、時刻ｔにおける式（７）から時刻（ｔ−ｔｓ）における式（７）を引くことにより、
【００５４】
ΔＦｅｓｔ（ｔ）＝ＫｈΔｐｅｓｔ（ｔ） …式（８）
【００５５】
が得られる。これにより、外部環境の平衡点の項ｐ０（ｔ）を消去することができ、外部環境の剛性係数を推定が可能となる。
【００５６】
また、外部環境の位置の変動が高速度域における場合には、サンプリング時間における平衡点の移動を無視することができないため、速度変動項を考慮した式（４）もしくは、より正確にはロボットのコンプライアンスをも考慮した式（６）を用いる。なお、ロボット１の自由度が多いとセンサへ入り込むノイズが大きくなり正確な剛性係数の推定が困難になる。そこで、ローパスフィルタを用いて所定の周波数をカットオフし、逐次最小二乗法などを用いて外部環境の剛性係数を推定する。
【００５７】
位置算出手段２３は、このように推定された外部環境の剛性係数およびロボット１側に設定された慣性係数、粘性係数、剛性係数を用いてエンドエフェクタ１１の位置を算出する。このロボット１側の慣性係数、粘性係数、剛性係数などのインピーダンス特性はパーソナルコンピュータ２ａに設けられた図示しない入力手段によって予め設定される。本実施の形態においては、慣性係数および剛性係数を固定値とし、粘性係数を外部環境の剛性係数と相関関係をもった可変のものとする。具体的には、図３に示すように外部環境の剛性係数Ｋｈの増大に比例して粘性係数Ｄも増大しうるような関係をもたせる。このようにすれば、例えば、人間とロボット１とが協働して重量物を運搬するような場合に、人間がしっかりとその運搬物を持つことによってロボット１側もその動きに硬さを持たせることができるようになり、また、逆に、軽量物を運搬するような場合、人間側のしなやかな運動に伴ってロボット１側もしなやかに追従することができるようになる。
【００５８】
制御手段２４は、サーボドライバ２ｂなどのようにロボット１のモータ１３ａを駆動するための手段を具備して構成され、位置算出手段２１によって算出された位置ｐにエンドエフェクタ１１が位置するようフィードバック制御される。
【００５９】
次に、ロボット１の制御方法について図２のブロック線図を用いて説明する。
【００６０】
まず、パーソナルコンピュータ２ａからの指令により、エンドエフェクタ１１に取り付けられた力覚センサ１２からサンプリング時間毎にセンシングデータを収集するとともに、ロボット１のモータ１３ａに取り付けられたエンコーダ１４からサンプリング時間毎にセンシングデータを収集する。そして、これらのデータをもとに外部環境の力差分値およびエンドエフェクタ１１の位置差分値を算出する。なお、この算出されたエンドエフェクタ１１の位置については、補正ルーチンで、まず、平衡点の移動補正を行い（式（３））、さらに、ロボットのコンプライアンスによる位置誤差の補正処理も行う（式（６））。そして、ローパスフィルタを用いてこれらの力差分値および位置差分値からノイズを除去した後、逐次最小二乗法を用いて外部環境の剛性係数Ｋｈの推定を行う。そして、この算出された剛性係数Ｋｈおよび、予め固定されたロボット１側の慣性係数、剛性係数、および外部環境の剛性係数と比例して増大する粘性係数を用いてエンドエフェクタ１１の目標位置ｐを算出する。そして、この目標位置にエンドエフェクタ１１を移動させるべく、サーボドライバ２ｂにモータ１３ａの角速度（ｄθ／ｄｔ）を目標値として出力し、サーボドライバ２ｂはこれに基づいてロボット１に動作指令値を出力する。また、このロボット１の動作に基づいてエンドエフェクタ１１の位置、あるいは関節角度がデータとしてフィードバックされ、これに基づいて更にエンドエフェクタ１１の位置が制御される。
【００６１】
このように本実施の形態によれば、エンドエフェクタ１１に作用する外部環境の力変動の時間差分と、その外部環境の作用位置変動の時間差分とからその外部環境の剛性係数を算出するとともに、予めロボット１側に設定された慣性係数および粘性係数および剛性係数、および前記算出された外部環境の剛性係数を用いてエンドエフェクタ１１の位置を算出してロボット１を駆動制御するようにしたため、人間とロボット１とが協働作業するような場合において、人間の動作に対応したリアルタイムなロボット１の制御が可能となる。
【００６２】
また、位置の時間差分を計測する場合、エンドエフェクタ１１の移動速度を考慮するようにしたので、サンプリング時間によって平衡点の移動を無視できないような場合であってもその移動速度を考慮した補正項によって正確に剛性係数を推定することができるようになる。
【００６３】
さらに、位置の時間差分を計測する場合、ロボット１のコンプライアンスを考慮するようにしたので、より正確な剛性係数の算出が可能になる。
【００６４】
加えて、外部環境の剛性係数を算出する場合、ローパスフィルタによって力差分値および位置差分値からノイズを除去して剛性係数を算出するようにしたため、自由度の増大に伴うノイズの影響を少なくして正確な剛性係数を算出することができるようになる。
【００６５】
また、外部環境の剛性係数を算出する場合、ロボット１側の粘性係数を、その外部環境の剛性係数と相関関係をもって変化させるようにして算出するようにしたため、人間の力加減に対応したロボット１の駆動制御を実現することができるようになる。
【００６６】
また、この粘性係数を、外部環境の剛性係数の増大に伴って粘性係数を大きくさせるように設定したため、人間とロボット１とが協働して重量物を運搬するような場合、外部環境の剛性係数の増大に伴ってロボット１側の動きを硬いものにすることができ、また、逆に、軽量物を迅速に運搬するような場合は、その人間側の剛性係数の減少に伴ってロボット１側の動きをしなやかにすることができる。
【００６７】
なお、本発明は上記実施の形態に限定されることなく、種々変更することができる。例えば、上記実施の形態においては、ロボット側のインピーダンス特性として粘性係数を外部環境側の剛性係数に比例して変化させるようにしたが、これに限らず、ロボット１側の慣性係数や剛性係数などについても相関関係を持たせて変化させ、これらに基づいて外部環境の剛性係数を算出するようにしても良い。また、その相関関係についても、比例関係だけに限らず、図４に示すような曲線状の相関関係を持たせるようにしても良い。
【００６８】
さらに、上記実施の形態においては、外部環境として人間を用いて説明したが、人間だけに限らず、机や壁などの障害物も外部環境として捉え、例えば、人間とロボット１とが運搬物を協働して運んでいる最中に、その運搬物が障害物に接触した場合も外部環境からの力として取り込むことができる。
【００６９】
【発明の効果】
本発明では、エンドエフェクタに作用する外部環境の力とその位置からアクチュエータを駆動制御する場合に、まず、エンドエフェクタに作用する外部環境の力の時間差分と、その外部環境の作用位置の時間差分とからその外部環境の剛性係数を算出し、そして、予め設定されたロボット側の慣性係数および粘性係数および剛性係数、および前記算出された外部環境の剛性係数を用いてエンドエフェクタの位置を算出してロボットのアクチュエータを駆動制御するようにしたので、差分時間を細かく設定することによって外部環境の平衡点に関する項を無視することができ、簡単にエンドエフェクタの位置を求めることができるようになる。
【図面の簡単な説明】
【図１】本実施の形態におけるインピーダンス制御装置の機能ブロック図
【図２】同形態におけるブロック線図
【図３】同形態における外部環境の剛性係数とロボット側の粘性係数の相関関係を示す図
【図４】第二の実施態における外部環境の剛性係数とロボット側の粘性係数の相関関係を示す図
【符号の説明】
１・・・ロボット
２・・・インピーダンス制御装置
１１・・・エンドエフェクタ
２０・・・力差分計測手段
２１・・・位置差分計測手段
２２・・・剛性係数算出手段
２３・・・位置算出手段
２４・・・制御手段
１００・・・ロボットシステム[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a control device for a robot that realizes impedance control, and in particular, an impedance that estimates a stiffness coefficient of an external environment in real time with respect to a force from the external environment acting on the robot and drives and controls a robot arm. It relates to a control device and the like.
[0002]
[Prior art]
As one method of controlling the movement of the robot, a method known as impedance control is known. This impedance control realizes the apparent softness of the robot by the external force generated in the end effector of the robot. When the external force is F and the position of the end effector is p, the following equation is used. Is achieved.
[0003]
Mp ″ + Dp ′ + Kp = F Equation (a)
[0004]
Here, M represents an inertia coefficient, D represents a viscosity coefficient, and K represents a stiffness coefficient, and is predetermined when controlling each part of the robot. Also, p ″ indicates acceleration and p ′ indicates speed.
[0005]
On the other hand, the force F of the external environment acting on the end effector of this robot follows the Hooke's law in a simplified equation,
[0006]
F = K h (p−p 0 ) Equation (b)
[0007]
Is represented by Here, K h stiffness coefficient of the external environment acts on the end effector, p is the position of the external environment, such as a human hand in contact with the end effector (i.e., the end effector position), p 0 is the external force of the external environment Indicates an equilibrium point at which is zero. Then, from equations (a) and (b),
[0008]
Mp ″ + Dp ′ + Kp = K h (p−p 0 ) Equation (c)
[0009]
Is satisfied, the position p of the end effector is calculated from the equation (c), and each part of the robot is controlled so as to move the end effector to that position.
[0010]
By the way, for example, when a robot and a human cooperate to carry a heavy object, the external force of the human being as the external environment and the equilibrium point of the force constantly change. Therefore, the equation (c) alone cannot calculate the end effector position p because there are three unknowns (the end effector position p, the equilibrium point position p 0 , and the elastic coefficient K h of the external environment). .
[0011]
For this reason, conventionally, a method of measuring the impedance characteristic (K h ) of the hand by manipulating the robot with the hand holding the end effector and fixing the hand position at a fixed position, or (“ Estimation of human hand impedance "(Transactions of the Society of Instrument and Control Engineers, 30-3, 1994, P319-328). By measuring the impedance characteristic (K h ) of the hand, p of equation (c) can be calculated (“Measurement of human arm opportunity impedance during multiple indirect movements in a horizontal plane” (Transactions of the Society of Instrument and Control Engineers 23). -3, 1996, P369-378), "Analysis of human hand impedance in isometric muscle contraction" (Transactions of the Society of Instrument and Control Engineers 32-2, 1). 996, P271-280).
[0012]
[Problems to be solved by the invention]
However, in the methods described in these documents, it is necessary to forcibly move the robot while the human hand is stationary or at a constant speed, and when the robot and the human collaborate in real time, the The forced movement of the robot disturbs human work.
[0013]
Further, according to the methods of these documents, the force F of the external environment is expressed as follows.
F = M h p "+ D h p '+ K h p
[0015]
In order to determine the inertia coefficient M h , viscosity coefficient D h , and stiffness coefficient K h on the external environment side, the position information is differentiated twice, but the robot has many degrees of freedom. When the position information is obtained by using, the influence of noise increases as the degree of freedom increases. This is because information on all joint angles possessed by the robot must be used in order to determine the end effector of the robot. In general, noise increases each time a differentiation is performed. Therefore, the methods described in these documents cannot be applied when a robot having multiple degrees of freedom is used.
[0016]
Further, in the method of these references, to estimate the impedance characteristic of the end effector after the measurement of the external force and the position, in particular, to estimate the K h retrieves data only forcibly added perturbation moiety subsequent measurement due to the way, it can not be performed K h estimated in real time during the cooperation work between humans and robots.
[0017]
In view of the above, the present invention has been made in view of the above-described problem, and estimates the stiffness coefficient (K h ) of the external environment in real time without forcibly moving the hand of a human, and highly accurately adjusts the robot. An object of the present invention is to provide an impedance control device capable of controlling each part.
[0018]
[Means for Solving the Problems]
The present invention, in order to solve the above problems, when driving and controlling the actuator from the position of the force of the external environment acting on the end effector, first, the time difference of the force fluctuation of the external environment acting on the end effector, The time difference of the action position fluctuation of the external environment is measured, and the stiffness coefficient of the external environment is calculated using these difference values. Then, the movement position of the end effector is calculated using the inertia coefficient, viscosity coefficient, and rigidity coefficient, which are preset impedance characteristics on the robot side, and the calculated rigidity coefficient of the external environment, and the position of the end effector is calculated. Information is output to drive and control the actuator of the robot.
[0019]
With this configuration, for example, by setting short differential time, it is possible to ignore the equilibrium point p 0 of the external environment, wherein the (a) [0020]
ΔF est (t) = K h Δp est (t)
[0021]
It can be ignored claim p 0 about the equilibrium point to approximate. Then, the F, determine the K h by sensing the p for each differential time, also, the end effector by using the this K h, the inertia coefficient preset robot side M, viscosity coefficient D, and the stiffness coefficient K Can be obtained.
[0022]
When measuring the time difference of the position change of the end effector, the position difference value is measured in consideration of the moving speed of the end effector.
[0023]
Usually, when the end effector is moving at high speed zone, to speed along with this also the equilibrium point of the external environment movement, it is impossible to ignore the terms p 0 at the equilibrium point. However, if the calculation is performed including a term that takes into account the moving speed of the end effector when measuring the time difference of the position fluctuation, the problem of the movement of the equilibrium point in the high speed region can be solved.
[0024]
Furthermore, when measuring the time difference of the position fluctuation of the end effector, the position difference value is measured in consideration of the compliance of the robot.
[0025]
With this configuration, it is possible to correct a position error caused by the compliance of the robot, and to measure a more accurate time difference of the position change.
[0026]
In addition, when calculating the stiffness coefficient of the external environment, the measured force difference value and position difference value are filtered, and after removing noise, the stiffness coefficient is calculated.
[0027]
Normally, as the degree of freedom of the robot increases, the noise acting on the sensor increases. However, with such a configuration, since the noise is removed by a filter, more accurate calculation of the stiffness coefficient becomes possible.
[0028]
In such an invention, the impedance characteristic of the robot is changed in correlation with the calculated rigidity coefficient of the external environment, and the position of the end effector is calculated using these values.
[0029]
With this configuration, it is possible to change the hardness and flexibility of the movement of the robot in accordance with the magnitude of the force acting from the external environment, and to perform control that is more suitable for collaborative work with humans Will be able to
[0030]
Preferably, the correlation is such that the viscosity coefficient increases as the rigidity coefficient of the external environment increases.
[0031]
According to this configuration, for example, when a human and a robot cooperate to carry a heavy object, the movement of the robot is made harder as the rigidity coefficient (K h ) of the human increases. Can carry heavy objects firmly. Conversely, when a lightweight object is quickly transported, the movement of the robot can be supple with the decrease of the stiffness coefficient (K h ) of the human side, so that the cooperative work can be performed more quickly. Can be realized.
[0032]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram of a robot 1 and an impedance control device 2 according to the present embodiment, and FIG. 2 is a block diagram of impedance control. The impedance control device 2 includes a personal computer 2a, a servo driver 2b, and the like. The impedance control device 2 and the robot 1 are connected to form a robot system 100 as a whole.
[0033]
As is well known, the robot 1 includes an arm 10 having an end effector 11 (see FIG. 2) at a distal end thereof, a driving unit 13 including a motor 13a which is an actuator for driving the arm 10, and the like. And a force sensor 12 for measuring the force of the external environment acting on the motor 13a, an encoder 14 for detecting the rotation angle of the motor 13a, and the like.
[0034]
On the other hand, the impedance control device 2 includes a force difference measurement unit 20, a position difference measurement unit 21, a stiffness coefficient calculation unit 22, a position calculation unit 23, and a control unit 24.
[0035]
The force difference measuring means 20 samples the value of the external force obtained from the force sensor 12 provided in the vicinity of the end effector 11 every minute time, and obtains a force difference value ΔF est ( Calculate t).
[0036]
ΔF est (t) = F R (t) −F R (t−t s ) Equation (1)
[0037]
This sampling time is preferably set to such an extent that the movement of the equilibrium point of the external environment can be ignored. For example, the sampling time is set to t s = 0.15 sec .
[0038]
The position difference measuring means 21 outputs the rotation angle obtained from the encoder 14 or the position information obtained from the position sensor when a position detection sensor capable of detecting the position of the end effector 11 is provided at every minute time. Sampling is performed to calculate a position difference value Δp est (t) represented by Expression (2).
[0039]
Δp est (t) = p R (t) −p R (t−t s ) Equation (2)
[0040]
This sampling time is preferably set to the same time as the sampling time measured by the force difference measuring means, and is set to, for example, ts = 0.15 sec .
[0041]
If the position of the external environment fluctuates in the high-speed region, the movement of the equilibrium point during the sampling time cannot be ignored, and it is necessary to consider the correction of the fluctuation of the position of the equilibrium point caused by the speed. Therefore, the position of the end effector 11 of the robot 1 is corrected. First, assuming that the speed of the end effector 11 of the robot 1 is v R (t), the distance of the end effector 11 moving during the time difference t s is t s v R (t−t s ). Therefore, a correction for the speed fluctuation is performed with respect to Expression (2),
[0042]
Δp est (t) = p R (t) −p R (t−t s ) −t s v R (t−t s ) Equation (3)
[0043]
By doing so, the problem when the equilibrium point moves is solved.
[0044]
Further, an error occurs in the position (p R ) of the end effector 11 of the robot 1 due to the compliance of the robot 1. Therefore, the position error of the end effector 11 is obtained in real time, and thereby the position of the end effector 11 of the robot 1 is also corrected. The position error resulting from compliance is
[0045]
Δp c (t) = C end (t) F R (t) ... Equation (4)
[0046]
Required by Here, Delta] p c indicates a positional error caused by the compliance of the robot 1, also, C end (t) indicates the compliance matrix in the end effector 11 of the robot 1. If the position of the end effector 11 of the robot 1 is corrected using the position error obtained by the equation (4), and the corrected position is p * R (t),
[0047]
p * R (t) = p R (t) + Δp c (t) ... (5)
[0048]
It becomes. When the p R of formula (3) in consideration of a position error of compliance is also replaced with p * R, Equation (3) is,
[0049]
Δp est (t) = p * R (t) -p * R (t-t s) -t s v R (t-t s) ... (6)
[0050]
It becomes. Then, to estimate the stiffness count K h by passing Delta] p est (t) of such consideration the speed variation and compliance obtained formula (6) to the stiffness coefficient calculating means 22.
[0051]
The stiffness coefficient calculating means 22 calculates an external environment based on the force difference value ΔF est (t) (Equation (1)) and the position difference value ΔP est (t) (Equation (6)) thus measured. The stiffness coefficient (K h ) of a hand of a certain person is calculated. In general, if the equilibrium point of the external environment (external force becomes zero position) and a p 0, in accordance with Hooke's law, has the following relationship to the force F R and p R of the external environment.
[0052]
F R (t) = K h (p R (t) −p 0 (t)) Equation (7)
[0053]
Meanwhile, if the variation of the position of the external environment in the low speed range, within the sampling time, the movement of the equilibrium point can be assumed to remain small range (p 0 (t) ≒ p 0 (t-t s)) , The movement of the equilibrium point in the adjacent sampling time can be ignored. Therefore, by subtracting equation (7) at time (t−t s ) from equation (7) at time t,
[0054]
ΔF est (t) = K h Δp est (t) Equation (8)
[0055]
Is obtained. Thereby, the term p 0 (t) of the equilibrium point of the external environment can be eliminated, and the rigidity coefficient of the external environment can be estimated.
[0056]
Further, when the position of the external environment fluctuates in a high-speed region, the movement of the equilibrium point during the sampling time cannot be ignored. Equation (6) that also takes into account compliance is used. If the degree of freedom of the robot 1 is large, noise entering the sensor increases, and it is difficult to accurately estimate the rigidity coefficient. Therefore, a predetermined frequency is cut off using a low-pass filter, and the stiffness coefficient of the external environment is estimated using a sequential least squares method or the like.
[0057]
The position calculating means 23 calculates the position of the end effector 11 by using the estimated rigidity coefficient of the external environment and the inertia coefficient, viscosity coefficient and rigidity coefficient set on the robot 1 side. Impedance characteristics such as an inertia coefficient, a viscosity coefficient, and a stiffness coefficient of the robot 1 are preset by input means (not shown) provided in the personal computer 2a. In this embodiment, the inertia coefficient and the stiffness coefficient are fixed values, and the viscosity coefficient is a variable having a correlation with the stiffness coefficient of the external environment. Specifically, impart a relationship as can be increased viscosity coefficient D in proportion to the increase of the stiffness coefficient K h of the external environment, as shown in FIG. In this way, for example, in the case where a human and the robot 1 cooperate to carry a heavy object, the robot 1 also has rigidity in its movement by holding the carried object firmly. On the contrary, when a lightweight object is carried, the robot 1 can flexibly follow the flexible movement of the human side.
[0058]
The control means 24 includes means for driving the motor 13 a of the robot 1, such as the servo driver 2 b, and performs feedback control so that the end effector 11 is located at the position p calculated by the position calculation means 21. Is done.
[0059]
Next, a control method of the robot 1 will be described with reference to a block diagram of FIG.
[0060]
First, according to a command from the personal computer 2a, sensing data is collected at every sampling time from the force sensor 12 attached to the end effector 11, and sensing is performed at every sampling time from the encoder 14 attached to the motor 13a of the robot 1. Collect data. Then, a force difference value of the external environment and a position difference value of the end effector 11 are calculated based on these data. With respect to the calculated position of the end effector 11, in the correction routine, first, the movement correction of the equilibrium point is performed (Equation (3)), and further, a correction process of the position error due to the robot compliance is performed (Equation (3)). 6)). Then, to estimate the stiffness coefficient K h in the external environment using after removing the noise from these forces difference value and position difference value, the sequential method of least squares using a low-pass filter. Then, the target position p of the end effector 11 is calculated by using the calculated rigidity coefficient Kh and the previously fixed inertia coefficient and rigidity coefficient of the robot 1 and the viscosity coefficient which increases in proportion to the rigidity coefficient of the external environment. Is calculated. Then, in order to move the end effector 11 to this target position, the angular velocity (dθ / dt) of the motor 13a is output as a target value to the servo driver 2b, and the servo driver 2b outputs an operation command value to the robot 1 based on this. I do. The position of the end effector 11 or the joint angle is fed back as data based on the operation of the robot 1, and the position of the end effector 11 is further controlled based on the data.
[0061]
As described above, according to the present embodiment, the stiffness coefficient of the external environment is calculated from the time difference of the force fluctuation of the external environment acting on the end effector 11 and the time difference of the action position fluctuation of the external environment. Since the position of the end effector 11 is calculated using the inertia coefficient, viscosity coefficient, and rigidity coefficient set in advance on the robot 1 side and the calculated rigidity coefficient of the external environment, the robot 1 is driven and controlled. In a case where the robot 1 and the robot 1 work together, real-time control of the robot 1 corresponding to human motion can be performed.
[0062]
Further, when measuring the time difference between the positions, the moving speed of the end effector 11 is taken into consideration. Therefore, even when the movement of the equilibrium point cannot be ignored due to the sampling time, the correction term taking into account the moving speed is taken into account. Thus, the rigidity coefficient can be accurately estimated.
[0063]
Further, when measuring the time difference between the positions, the compliance of the robot 1 is taken into account, so that a more accurate calculation of the stiffness coefficient becomes possible.
[0064]
In addition, when calculating the stiffness coefficient of the external environment, the low-pass filter removes noise from the force difference value and the position difference value to calculate the stiffness coefficient, thereby reducing the influence of noise due to an increase in the degree of freedom. Thus, an accurate stiffness coefficient can be calculated.
[0065]
In addition, when calculating the rigidity coefficient of the external environment, the viscosity coefficient of the robot 1 is calculated so as to be changed in correlation with the rigidity coefficient of the external environment. Drive control can be realized.
[0066]
In addition, since the viscosity coefficient is set so as to increase as the rigidity coefficient of the external environment increases, when the human and the robot 1 cooperate to transport a heavy object, the rigidity of the external environment is increased. When the coefficient increases, the movement of the robot 1 can be made harder. Conversely, when a lightweight object is to be quickly conveyed, the robot 1 moves with a decrease in the rigidity coefficient of the human. The side movement can be supple.
[0067]
It should be noted that the present invention is not limited to the above embodiment, but can be variously modified. For example, in the above embodiment, the viscosity coefficient is changed in proportion to the rigidity coefficient on the external environment side as the impedance characteristic on the robot side. However, the present invention is not limited to this. May be varied with a correlation, and the stiffness coefficient of the external environment may be calculated based on these. Further, the correlation is not limited to the proportional relationship, but may have a curved correlation as shown in FIG.
[0068]
Furthermore, in the above-described embodiment, the description has been given using a human as the external environment. However, not only humans but also obstacles such as desks and walls are also considered as external environments. Even if the conveyed object comes into contact with an obstacle during cooperative transportation, it can be taken in as a force from the external environment.
[0069]
【The invention's effect】
In the present invention, when the actuator is driven and controlled based on the external environment force acting on the end effector and its position, first, the time difference between the external environment force acting on the end effector and the time difference between the external environment acting position From the above, the rigidity coefficient of the external environment is calculated, and the position of the end effector is calculated using the preset inertia coefficient, viscosity coefficient, and rigidity coefficient of the robot side, and the calculated rigidity coefficient of the external environment. Since the actuator of the robot is driven and controlled, the term relating to the equilibrium point of the external environment can be ignored by setting the difference time finely, and the position of the end effector can be easily obtained.
[Brief description of the drawings]
FIG. 1 is a functional block diagram of an impedance control device according to the present embodiment. FIG. 2 is a block diagram of the same embodiment. FIG. 3 is a diagram showing a correlation between a rigidity coefficient of an external environment and a viscosity coefficient of a robot in the same embodiment. FIG. 4 is a diagram showing a correlation between a stiffness coefficient of an external environment and a viscosity coefficient of a robot in the second embodiment.
DESCRIPTION OF SYMBOLS 1 ... Robot 2 ... Impedance control apparatus 11 ... End effector 20 ... Force difference measuring means 21 ... Position difference measuring means 22 ... Stiffness coefficient calculating means 23 ... Position calculating means 24 ... Control means 100 ... Robot system
