JP3972151B2

JP3972151B2 - 負荷トルクのオブザーバ

Info

Publication number: JP3972151B2
Application number: JP08581197A
Authority: JP
Inventors: 慎悟安藤; 満徳川辺
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1997-03-19
Filing date: 1997-03-19
Publication date: 2007-09-05
Anticipated expiration: 2017-03-19
Also published as: JPH10264057A

Description

【０００１】
【発明の属する技術分野】
本発明は、マニピュレータなどのメカニカルシステムの駆動軸に加わる外力（負荷トルクを含む）を推定して、その推定値を力制御系に利用する際の、外力の推定方法に関する。
【０００２】
【従来の技術】
従来、マニピュレータの力制御方式において、マニピュレータ先端の外力・モーメント、あるいは各関節に加わる外力・負荷トルクを直接計測するセンサを使用せずに、各関節アクチュエータへの指令入力と各関節の位置、速度、加速度などの出力信号を利用して、現代制御理論におけるオブザーバを構成することにより、各関節に加わる外力あるいは負荷トルクを推定し、その推定値に基づいてコンプライアンス制御、インピーダンス制御などの力制御系を構成する手法がある。例えば、特開平6-175705，文献「大石、宮崎、藤田：力センサを用いない位置と力のハイブリッド制御，日本ロボット学会誌，Vol.11, No.3, pp.468-476, 1993」、「則次，朴：外乱オブザーバを用いた空気圧マニピュレータの力制御，日本ロボット学会誌，vol.13, No.5, pp.711-719, 1995」などがある。これらの外力・負荷トルクオブザーバでは、外力（トルク）と静止摩擦、クーロン摩擦（動摩擦）の和を推定することになる。したがって、静止摩擦、クーロン摩擦といった非線形摩擦を補償する必要があるが、その具体的補償法については、あまり言及していない。その他の一般的な図書によれば、クーロン摩擦、最大静止摩擦を駆動軸の速度の関数として、図６のようにモデリングしている。
ここで、図５の１自由度マニピュレータを考える。
図５において、５０１、５０２、５０３、５０４、５０５はそれぞれ、アクチュエータ、アクチュエータ回転軸、減速機、リンク回転軸、リンクである。アクチュエータ回転軸、リンク回転軸の発生トルクをそれぞれ、τu,τe とする。ここで、アクチュエータは電磁式のサーボモータとし、サーボモータのドライバ回路には電機子電流のフィードバックループが構成されており、モータ発生トルクτu を直接制御できるものとする。減速機による共振を無視し、減速後のモータ回転角とリンク回転角が等しいと近似すると、図５の１自由度マニピュレータの運動方程式は、
【０００３】
【数１】

【０００４】
と表せる。ここで、Ｊm,ｂm,θ, τfricはそれぞれ、マニピュレータの慣性モーント、減速機の粘性摩擦係数、リンク回転角、減速機の非線形摩擦である。式（１）による従来の負荷トルクオブザーバは図３のように表せる。図３において、３０１、３０２はそれぞれ、１自由度マニピュレータのダイナミクス、負荷トルクオブザーバである。３０３、３０４はそれぞれ、リンク駆動トルクからリンク回転速度までの伝達関数、積分器である。３０５ａ、３０５ｂ、３０５ｃはローパスフィルタ、３０６は３０３のダイナミクス補償器である。つまり、マニピュレータに供給されるトルク入力から、リンクの慣性力、粘性摩擦に相当する分を差し引いている。図６(b) の非線形摩擦は、
【０００５】
【数２】

【０００６】
のように式で表せる。３０７は式（２）に基づく非線形摩擦τfricの補償器である。また、式（２）におけるτc,τs.max および３０３のＪm 〜, ｂm 〜は、それぞれパラメータ同定により得られるクーロン摩擦、最大静止摩擦、マニピュレータの慣性モーメント、粘性摩擦係数の公称値である。τe ＾が負荷トルクの推定値である。
ここで、図５のモータ固定子は作業空間に固定されているものとする。リンク５０５の先端も作業空間に固定された剛性の非常に高い環境と接触しており、リンクは環境の方向へは回転しないものとする。このとき、モータ発生トルクからリンク発生トルクへのトルク伝達特性は、図４のようなヒステリシス特性を示す。式（１）のように１自由度マニピュレータをモデリングした場合、リンクは回転しないので、負荷トルクの推定値は、モータ発生トルクと等しくなる。そのため、トルク伝達のヒステリシス特性により、リンク静止時の負荷トルク推定誤差は大きくなる。実際には、減速機のコンプライアンスにより、リンク回転軸が回転しなくても、モータ回転角はスティック・スリップを起こしながら微少に変動している。したがって、モータ回転角を計測することにより、ある程度リンク静止時の負荷トルク推定誤差を軽減できるように思われるが、モータがスリップしたときのみ式（２）の非線形摩擦補償が作動するため、ほとんどヒステリシス特性を補償することはできない。また、モータがスリップした際の回転速度は非常に微少であるため、式（２）における符合関数sgn を精度良く計算できない。減速機のコンプライアンスによるダイナミクスを考慮したモデルを用いて、負荷トルクオブザーバを構成することにより、ヒステリシス特性をある程度補償できるようになるが、モータのスリップの際の回転角、回転速度は微少であり、その補償はモータ回転角の検出精度に非常に影響され、先に述べたように符合関数sgn も精度良く計算できない。したがって、従来の式（２）に基づく非線形摩擦補償器では、図４のヒステリシス特性をうまく補償できず、リンク静止時の負荷トルク推定誤差が大きくなる。
【０００７】
【発明が解決しようとする課題】
駆動リンク静止時におけるアクチュエータ発生力（トルク）から出力軸における発生力（トルク）への力（トルク）伝達には、通常ヒステリシス特性がある。特に、駆動軸に減速機構を有する場合は、そのメカニカルなコンプライアンスのために、アクチュエータ発生力（トルク）と減速後の発生力（トルク）のヒステリシス特性は顕著になる。式（２）のように非線形摩擦をモデリングした場合、ヒステリシス特性をうまく補償できないため、駆動軸静止時における外力（トルク）推定誤差が大きくなるという問題がある。
そこで本発明は、駆動軸静止時の外力（トルク）推定精度が向上し、精度の良い力制御系が実現できる方法を提供することを目的とする。
【０００８】
【課題を解決するための手段】
上記課題を解決するため、本発明では、減速機構を有するサーボモータと、前記減速機構に回転されるリンクと、からなるマニピュレータの各駆動軸における前記サーボモータへの指令トルクと、該指令トルクから算出される前記リンクの回転速度と、を用いて、前記各駆動軸における前記リンクに加わる負荷トルクを推定するオブザーバにおいて、前記オブザーバが、前記リンク回転速度を監視して、前記リンクが静止していると判断したとき、１サンプリング時刻前の前記サーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値から前記駆動軸の最大静止摩擦を減算した値よりも大きく、かつ前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値よりも小さいとき、現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値として出力し、前記１サンプリング時刻前の前記サーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値以上であり、かつ前記１サンプリング時刻前の前記サーボモータへの指令トルクの増分値が正のとき、前記現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値から前記駆動軸の最大静止摩擦を減算した値として出力し、前記１サンプリング時刻前のサーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を減算した値以下であり、かつ前記１サンプリング時刻前の前記サーボモータへの指令トルクの増分値が負のとき、前記現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値として出力する、ヒステリシス特性補償器を備えたことを特徴とする負荷トルクのオブザーバとするものである。
【０００９】
【発明の実施の形態】
以下、本発明の実施の形態を図１，２により説明する。
図１は、本発明における負荷トルク推定器の制御構成図を表す。図１において１０１、１０２はそれぞれ、１自由度マニピュレータのダイナミクス、本発明による負荷トルクオブザーバである。１０３、１０４はそれぞれ、リンク駆動トルクからリンク回転速度までの伝達関数、積分器である。１０５ａ、１０５ｂ、１０５ｃはローパスフィルタ、１０６は１０３のダイナミクス補償器である。つまり、マニピュレータに供給されるトルク入力から、リンクの慣性力、粘性摩擦に相当する分を差し引いている。１０７は、
【００１０】
【数３】

【００１１】
によりクーロン摩擦のみを補償するクーロン摩擦補償器である。１０８は、本発明による駆動軸静止時のヒステリシストルク伝達特性の補償器である。１０９はリンク回転速度を監視して、回転時と静止時の負荷トルク推定方式の切り換を行う。
【００１２】
【数４】

【００１３】
のときは、リンクが回転しているとして、１０６、１０７により、リンクの慣性、粘性摩擦、クーロン摩擦を補償して負荷トルクを推定する。
【００１４】
【数５】

【００１５】
のときは、リンクが静止している、もしくは、スティックスリップの状態と判断し、１０８のヒステリシス特性補償器のみにより、負荷トルクを推定する。τe＾が負荷トルクの推定値である。図２は、１０８のヒステリシス特性補償器のアルゴリズムを表している。図２において、まず２０１により、テンポラリ変数の演算を行う。２０１において、１サンプリング前の時刻の負荷トルク推定値τe ＾(k-1) をτtmp 、１サンプリング前の時刻のモータへの指令トルクτu(k-1)をτup、１サンプリング前の時刻のモータへの指令トルクの増分値τu(k-1) -τu(k-2)をτudとおく。２０２は第１の判断部であり、
τtmp −τs.max ＜τup＜τtmp ＋τs.max （６）
のとき、２０５により、現時刻の負荷トルクを
τe ＾(k) ＝τtmp （７）
と計算する。２０３は第２の判断部であり、
τup≧τtmp ＋τs.max かつτud＞０（８）
のとき、２０６により、現時刻の負荷トルクを
τe ＾(k) ＝τup − τs.max （９）
と計算する。２０４は第３の演算部であり、
τup ≦τtmp −τs.max かつτud＜０（１０）
のとき、２０７により、現時刻の負荷トルクを
τe ＾(k) ＝τup＋τs.max （１１）
と計算する。２０８は遅延要素であり、１サンプリング信号を遅らせる作用を持つ。式（６），（８），（１０）のいづれの条件も満たさない場合は、モータが回転している場合である。以上のアルゴリズムにより、図４のヒステリシストルク伝達特性を補償することにより、駆動軸静止時における負荷トルク推定精度を向上させることが可能となる。
【００１６】
【発明の効果】
以上述べたように、本発明によれば、駆動軸静止時におけるアクチュエータ発生力（トルク）から出力軸発生力（トルク）までの力（トルク）伝達ヒステリシス特性に起因する外力（トルク）推定誤差を、上記ヒステリシス特性の数学モデルとアクチュエータへの指令力（トルク）入力および外力（トルク）推定値の過去から現時刻に至るまでの履歴を用いて補償することにより、駆動軸静止時の外力（トルク）推定精度が向上し、精度の良い力制御系が実現できるという効果がある。
【図面の簡単な説明】
【図１】本発明実施の形態における外力・負荷トルク推定方式の制御ブロック図
【図２】本発明実施の形態における外力・負荷トルク推定方式のヒステリシストルク伝達特性補償器のアルゴリズムを表す図
【図３】従来の外力・負荷トルク推定方式の制御ブロック図
【図４】駆動軸静止時におけるトルク伝達のヒステリシス特性を表す図
【図５】減速機構を有する１自由度マニピュレータの概形図
【図６】従来の非線形摩擦のモデル図
【符号の説明】
１０１１自由度マニピュレータのダイナミクス
１０２本発明の負荷トルク推定器
１０３モータ指令トルクからリンク回転速度までの伝達関数
１０４積分器
１０５ａローパスフィルタ
１０５ｂローパスフィルタ
１０５ｃローパスフィルタ
１０６慣性力、粘性摩擦のダイナミクス補償器
１０７クーロン摩擦の補償器
１０８ヒステリシストルク伝達特性の補償器
１０９リンク回転時、静止時の負荷トルク推定方式の切り換部
２０１テンポラリ変数の演算部
２０２第１の判断部
２０３第２の判断部
２０４第３の判断部
２０５第１の負荷トルクの推定部
２０６第２の負荷トルクの推定部
２０７第３の負荷トルクの推定部
２０８１サンプリングの信号の遅延要素
３０１１自由度マニピュレータのダイナミクス
３０２従来の負荷トルク推定器
３０３モータ指令トルクからリンク回転速度までの伝達関数
３０４積分器
３０５ａローパスフィルタ
３０５ｂローパスフィルタ
３０５ｃローパスフィルタ
３０６慣性力、粘性摩擦のダイナミクス補償器
３０７従来の非線形摩擦の補償器
５０１アクチュエータ
５０２アクチュエータ回転軸
５０３減速機
５０４リンク回転軸
５０５リンク

Claims

減速機構を有するサーボモータと、前記減速機構に回転されるリンクと、からなるマニピュレータの各駆動軸における前記サーボモータへの指令トルクと、該指令トルクから算出される前記リンクの回転速度と、を用いて、前記各駆動軸における前記リンクに加わる負荷トルクを推定するオブザーバにおいて、
前記オブザーバが、
前記リンク回転速度を監視して、前記リンクが静止していると判断したとき、
１サンプリング時刻前の前記サーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値から前記駆動軸の最大静止摩擦を減算した値よりも大きく、かつ前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値よりも小さいとき、現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値として出力し、
前記１サンプリング時刻前の前記サーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値以上であり、かつ前記１サンプリング時刻前の前記サーボモータへの指令トルクの増分値が正のとき、前記現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値から前記駆動軸の最大静止摩擦を減算した値として出力し、
前記１サンプリング時刻前のサーボモータへの指令トルクが、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を減算した値以下であり、かつ前記１サンプリング時刻前の前記サーボモータへの指令トルクの増分値が負のとき、前記現時刻の負荷トルクを、前記１サンプリング時刻前の負荷トルク推定値に前記駆動軸の最大静止摩擦を加算した値として出力する、ヒステリシス特性補償器を備えたことを特徴とする負荷トルクのオブザーバ。