JP5919143B2 - 駆動装置 - Google Patents

Description

本発明は、ロボットの関節を駆動させる駆動装置に関する。

下記特許文献１には、駆動系の根本から手先側の先端部にかけて設けた３個のリンク部材を３個の関節で連結し、一番根本の駆動部には駆動用アクチュエータに固定された３つのプーリが設置され、各関節の関節軸には、そこを経由するワイヤの本数と同数のプーリを独立して回転が可能に設置することで、ロボットアームが外部環境と接触した際に、駆動用アクチュエータに固定されたプーリと各ワイヤとが滑りを生じ、外部環境に過大な力を加えないようにするロボットアーム駆動装置が記載されている。

特開平１０−２４９７７７号公報

しかしながら、関節に外力が与えられてしまうと、実際に関節にかかるトルクは、モータ等のアクチュエータによって関節に加えられるトルクと異なってしまい、この実際に関節に加えられたトルクが目標トルクとなるようにアクチュエータを制御してしまうので、関節の駆動制御を適切に行うことができない。

そこで本発明は、関節等の被回動部材に外力が与えられた場合であっても、被回動部材を適正に制御することができる駆動装置を提供することを目的とする。

本発明は、駆動装置であって、回転駆動源と、前記回転駆動源の出力側に連結される駆動連結部材と、前記駆動連結部材の出力側に連結される第１部材と、関節である被回動部材が連結された第２部材と、前記第１部材の回転力を第２部材に伝達する弾性部材と、前記第１部材及び前記第２部材の変位量を検出する変位量検出手段と、前記第２部材の前記弾性部材に対する滑り量を算出する滑り量算出部と、前記滑り量に基づいて前記回転駆動源の出力トルクを制御する回転制御部と、を備え、前記回転駆動源が駆動される前の前記弾性部材の引っ張り力、前記弾性部材の全長、前記第２部材と前記弾性部材との静止摩擦係数、前記第１部材及び前記第２部材間の距離、前記第１部材及び前記第２部材の半径、及び前記駆動連結部材の１：１を含む減速比に基づいて、前記第１部材が前記弾性部材に対して滑り始める滑り開始トルクが設定されており、前記回転制御部は、前記滑り開始トルク未満のトルクを出力するように前記回転駆動源の出力トルクを制御し、前記滑り量算出部は、前記第１部材の変位速度の変化量、前記第１部材と前記第２部材との変位量差、及び前記回転駆動源の出力トルクに基づいて前記滑り量を算出することを特徴とする。

前記駆動装置において、前記滑り量算出部は、下記の関係式によって、滑り量を算出してもよい。
Ｓ＝Ｉ１／ｋｒ・（ｄθ１（ｎ＋１）−ｄθ１（ｎ））／ｄｔ＋（θ２（ｎ）−θ１（ｎ））−τｒ（ｎ）／ｋｒ
（但し、Ｓ：前記滑り量、 Ｉ１：前記第１部材、前記駆動連結部材、及び前記回転駆動源の慣性モーメント、 ｋｒ：前記第１部材及び前記第２部材間のねじりばね定数、 θ１（ｎ）：タイミングｎで検出された第１部材の変位量、 θ２（ｎ）：タイミングｎで検出された第２部材の変位量、ｄθ１（ｎ）：タイミングｎで検出された第１部材の変位速度の変化量、 τｒ（ｎ）：タイミングｎで検出された駆動連結部材の出力トルク）

前記駆動装置において、前記滑り量算出部は、算出した滑り量にローパスフィルタ処理を施して該滑り量を補正してもよい。

本発明によれば、第１部材の変位速度の変化量、第１部材と第２部材との変位量差、及び回転駆動源の出力トルクに基づいて滑り量を算出するので、回転駆動源から関節である被回動部材に加わる実際のトルクを求めることができ、関節の変位を加速度、速度、位置の次元で適切に制御することができる。

また、回転駆動源が駆動される前の弾性部材の引っ張り力、弾性部材の全長、第２部材と弾性部材との静止摩擦係数、第１部材及び第２部材間の距離、第１部材及び第２部材の半径、及び駆動連結部材の減速比に基づいて、第１部材が弾性部材に対して滑り始める滑り開始トルクが設定されており、回転制御部は、滑り開始トルク未満のトルクを出力するように回転駆動源の出力トルクを制御するので、回転駆動源の駆動によって第１部材が弾性部材に対して滑ることを防止することができる。

実施の形態に係る駆動装置の全体構成図である。 ワイヤに要求される強度設定を説明するための図である。 制御部の電気的な概略構成図である。

本発明に係る駆動装置について、好適な実施の形態を掲げて添付の図面を参照しながら以下詳細に説明する。

図１は、実施の形態に係る駆動装置１０の全体構成図である。駆動装置１０は、モータ１２、駆動連結部材１４、第１プーリ１６、ワイヤ１８、第２プーリ２０、被回動部材２２、及び制御部２４を備える。

モータ１２（回転駆動源）は、いわゆる電動機であり、図示しない電源から電力が供給されることで、モータ１２の出力軸１２ａ（モータ軸）を軸周りに回転させるトルクを発生する。この出力軸１２ａには、該出力軸１２ａと一体に回転し得るように駆動連結部材１４の入力軸１４ａが連結されている。駆動連結部材１４は、入力軸１４ａに入力された回転力を所定の減速比で減速して出力軸１４ｂから出力する。駆動連結部材１４の出力軸１４ｂは、第１プーリ（第１部材）１６の入力軸１６ａに連結されている。なお、駆動連結部材１４は、入力軸１４ａから入力された回転力を減速させることなく（減速比が１：１）、出力軸１４ｂに伝達させるものであってもよい。つまり、モータ１２の回転力を第１プーリ１６に伝達するものであればよい。

ワイヤ１８は、円環状の弾性部材であり、第１プーリ１６の回転力を第２プーリ（第２部材）２０に伝達するように、第１プーリ１６及び第２プーリ２０に取り付けられている。第２プーリ２０の出力軸２０ａには、被回動部材２２が連結されている。

この駆動装置１０は、ロボットの関節に適用され、被回動部材２２が、例えば、ロボットの膝の関節として用いられる場合は、被回動部材２２が下腿部となり、モータ１２がロボットの大腿部に取り付けられる。

このワイヤ１８の全長や剛性、及び、第１プーリ１６及び第２プーリ２０の半径等によって、駆動装置１０が適用される関節の剛性を変えることができる。

変位エンコーダ（回転角度検出手段、変位量検出手段）２６は、モータ１２の回転角度（変位量）θｍを検出し、変位エンコーダ（回転角度検出手段、変位量検出手段）２８は、第２プーリ２０の回転角度（変位量）θ２を検出する。また、トルクセンサ３０は、駆動連結部材１４が出力するトルクτｒを検出する。変位エンコーダ２６、変位エンコーダ２８、及びトルクセンサ３０が検出した信号（値）は、制御部２４に出力される。

なお、トルクセンサ３０をモータ１２に設け、モータ１２が出力する出力トルクτｍを検出してもよい。この場合、モータ１２の出力トルクτｍに駆動連結部材１４の減速比を乗算することで、駆動連結部材１４が出力するトルクτｒを算出することができる。また、制御部２４がモータ１２に供給する電流を検出する電流センサ（図示略）を設け、該電流センサが検出した電流にトルク定数（供給された電流に対する出力トルクの係数）を乗算することでモータ１２の出力トルクτｍを求めることもできる。

図２は、ワイヤ１８に要求される強度設定を説明するための図である。図２では、第１プーリ１６の半径をｒ（ｍ）、第２プーリ２０の半径をＲ（ｍ）とし、全長（周長）がＬ（ｍ）のワイヤ１８は、∞の字を描くように第１プーリ１６及び第２プーリ２０に取り付けられている。ｂ１は、ワイヤ１８が交差する点から、ワイヤ１８が第１プーリ１６に接触する点までの距離を表し、ｂ２は、ワイヤ１８が交差する点から、ワイヤ１８が第２プーリ２０に接する点までの距離を表している。また、図２においては、ワイヤ１８は、∞の字を描くように第１プーリ１６及び第２プーリ２０に取り付けられているが、これに限定されるものではなく、円環状となるように第１プーリ１６及び第２プーリ２０に取り付けられてもよい。

第１プーリ１６にトルクを付加して回転させた場合に、第２プーリ２０がワイヤ１８に対しての滑り始める時のトルク（滑り開始トルク）をＴｓ、滑り開始トルクＴｓが第２プーリ２０に付加された時のワイヤ１８の引っ張り力をＴｓ／Ｒ（Ｎ）、ワイヤ１８が第２プーリ２０の全周に巻かれている時のワイヤ１８と第２プーリ２０間の静止摩擦係数をμで表すと、第１プーリ１６が駆動される前からワイヤ１８に係る引っ張り力Ｆｐは、
Ｆｐ＝Ｌ・Ｔｓ・１／（２・Ｒ・μ・（Ｌ−２（ｂ１＋ｂ２）））
の関係式で表すことができる。従って、ワイヤ１８は、Ｔｓ／Ｒ＋Ｆｐが付加された状態において破壊されない程度の強度を有する必要がある。なお、本実施の形態では、「・」は乗算を表す。

また、上述した関係式から、引っ張り力Ｆｐ、ワイヤ１８の全長Ｌ、静止摩擦係数μ、第１プーリ１６及び第２プーリ２０間の距離、及び第２プーリ２０の半径Ｒによって、滑り開始トルクＴｓを設定することができる。また、第１プーリ１６のワイヤ１８に対する滑り開始トルクＴｓ１は、Ｔｓ１＝Ｔｓ・Ｒ／（ｒ×駆動連結部材１４の減速比）で表すことができるので、モータ１２の出力トルクを、Ｔｓ１未満にすることで、第１プーリ１６がワイヤ１８に対して滑らないように制御することができる。

なお、図２においては、第１プーリ１６の半径ｒを、第２プーリ２０の半径Ｒより小さくしているが、以下、本実施の形態の第１プーリ１６と第２プーリ２０との半径は同じとして説明する。

外部から被回動部材２２に力（外力）が加えられると、第２プーリ２０がワイヤ１８に対して滑るので、このような滑りの発生時には、実際に被回動部材２２にかかるトルク、つまり、第１プーリ１６及び第２プーリ２０間の回転角度差を用いて算出される被回動部材２２にかかるトルクは、外力とモータ１２から被回動部材２２に伝達される実トルクτａｃｔとの合算であるので、モータ１２から被回動部材２２に伝達される実トルクτａｃｔが目標トルクτｒｅｆとなるように制御することができない。

従って、制御部２４は、この滑った量（以下、滑り量）Ｓを求め、該求めた滑り量Ｓと実際に被回動部材２２にかかるトルクとから、モータ１２から被回動部材２２に伝達される実トルクτａｃｔを推定することで、被回動部材２２に伝達されるトルクが目標トルクτｒｅｆとなるように、モータ１２を駆動制御する。なお、本実施の形態では、第１プーリ１６がワイヤ１８に対して滑らないトルクで、モータ１２を制御しているので、原則として第１プーリ１６がワイヤ１８に対して滑らない。

以下、滑り量Ｓの算出手法について説明する。第１プーリ１６の運動方程式は、
α１＝τ１/Ｉ１ ・・・数式（１）
で表すことができる。α１は、第１プーリ１６の回転角加速度であり、τ１は、第１プーリ１６であり、Ｉ１は、第１プーリ１６、駆動連結部材１４、及びモータ１２の慣性モーメントである。

この回転角加速度α１は、
α１＝（ｄθ１（ｎ＋１）−ｄθ１（ｎ））/ｄｔ ・・・数式（２）
で表される。θ１（ｎ＋１）は、タイミングｎ＋１（例えば、今回の検出タイミング）で検出された第１プーリ１６の回転角度（変位量）を表し、θ１（ｎ）は、タイミングｎ（例えば、前回の検出タイミング）で検出された第１プーリ１６の回転角度を表しているので、ｄθ１（ｎ＋１）は、タイミングｎ＋１時の第１プーリ１６の回転角速度（変位速度）、ｄθ１（ｎ）は、タイミングｎ時の第１プーリ１６の回転角速度を表している。また、ｄｔは、サンプリング時間を表している。

このｄθ１（ｎ＋１）は、ｄθ１（ｎ＋１）＝（θ１（ｎ＋１）−θ１（ｎ））／ｄｔ、の関係式で表すことができ、ｄθ１（ｎ）は、ｄθ１（ｎ）＝（θ１（ｎ）−θ１（ｎ−１））／ｄｔ、の関係式で表すことができる。

第１プーリ１６の回転角度θ１は、変位エンコーダ２６が検出したモータ１２の回転角度θｍを駆動連結部材１４の減速比で除算することで求めることができる。また、第１プーリ１６の回転角速度ｄθ１は、第１プーリ１６の回転角度θ１を微分することで求めることもできる。

また、トルクτ１は、駆動連結部材１４からのトルクτｒと、第２プーリ２０から第１プーリ１６に掛かるトルクとを合算したものであり、第２プーリ２０の滑り量Ｓを考慮すると、トルクτ１は、
τ１＝ｋｒ・（θ１（ｎ）−（θ２（ｎ）−Ｓ））＋τｒ（ｎ） ・・・数式（３）
で表される。ｋｒは、第１プーリ１６及び第２プーリ２０間のねじりばね定数（Ｎ・ｍ／ｒａｄ）であり、関節の剛性を表している。θ２（ｎ）は、タイミングｎで検出された第２プーリ２０の回転角度を表している。駆動連結部材１４からのトルクτｒ（ｎ）は、タイミングｎ時に検出された駆動連結部材１４からのトルクτｒである。駆動連結部材１４からのトルクτｒは、トルクセンサ３０によって検出することができ、第２プーリ２０の回転角度θ２は、変位エンコーダ２８によって検出することができる。

従って、数式（１）、（２）、（３）とから、
Ｓ＝Ｉ１／ｋｒ・（ｄθ１（ｎ＋１）−ｄθ１（ｎ））／ｄｔ＋（θ２（ｎ）−θ１（ｎ））−τｒ（ｎ）／ｋｒ ・・・数式（４）
を導くことができる。数式（４）の第１項の「（ｄθ１（ｎ＋１）−ｄθ１（ｎ））／ｄｔ」は、第１プーリ１６の回転角速度ｄθ１の変化量（つまり、回転角加速度（変位加速度）α）を表し、第２項は、第１プーリ１６と第２プーリ２０との回転角度差（変位量差）を表し、第３項の「τｒ（ｎ）」は、駆動連結部材１４が出しているトルクであるから、滑り量Ｓは、第１プーリ１６の回転角速度ｄθ１の変化量、第１プーリ１６と第２プーリ２０との回転角度差、及び駆動連結部材１４が出力するトルクτｒ（モータ１２の出力トルクτｍ）によって変動する。

外部から被回動部材２２に力が加えられることにより（外乱により）、第２プーリ２０がワイヤ１８に対して滑る場合は、第１プーリ１６の回転角速度ｄθ１の変化は殆どなく、トルクτｒも殆ど変化しない。従って、第１プーリ１６と第２プーリ２０との回転角度差が変化し、主に低周波で変化する。

第１プーリ１６の滑りは、モータ１２の制御において、第１プーリ１６の静止摩擦限界を超える力を出してしまった場合を意味しており、静止摩擦限界が既知であれば、第１プーリ１６の滑りが発生しないように、モータ１２を制御することも可能であるから、上述したように原則として第１プーリ１６は滑らない。しかしながら、モータ１２と第１プーリ１６とは駆動連結部材１４を介して接続されているので、駆動連結部材１４の機械的な特性（例えば、ガタ等）によって、第１プーリ１６がワイヤ１８に対して若干滑る可能性がある。この第１プーリ１６が滑ると、第１プーリ１６の回転角速度ｄθ１の変化量及びトルクτｒは、高周波で大きく変化し、第１プーリ１６と第２プーリ２０との回転角度差は、中周波で変化する。また、滑り量Ｓを算出するために必要なセンサである変位エンコーダ２８等は、分解能が悪く、高周波で変化する場合に対応することができない。このような観点から、本実施の形態では、数式（４）で得られた滑り量Ｓに、ローパスフィルタ処理を施すことで滑り量Ｓを補正する。

図３は、制御部２４の電気的な概略構成図である。制御部２４は、滑り量算出部５０と、モータ制御部（回転制御部）５２とを有する。モータ制御部５２は、トルク値算出部６０、演算部６２、６４、６６、ローパスフィルタ６８、ＰＤ制御部７０、及びドライバ７２を備える。制御部２４には、モータ１２から被回動部材２２に付与すべき目標トルクτｒｅｆが外部の図示しない制御装置から逐次入力される。

滑り量算出部５０には、変位エンコーダ２６が検出したモータ１２の回転角度θｍ、変位エンコーダ２８が検出した第２プーリ２０の回転角度θ２、トルクセンサ３０が検出したトルクτｒから、数式（４）を用いて滑り量Ｓを算出（推定）し、該算出した滑り量Ｓにローパスフィルタ処理を施すことで、滑り量Ｓを補正する。なお、回転角度θ１は、モータ１２の回転角度θｍを駆動連結部材１４の減速比で除算することで求めることができる。

トルク値算出部６０は、モータ１２から被回動部材２２（第２プーリ２０）に付与される実トルクτａｃｔを、第１プーリ１６の回転角度θ１、第２プーリ２０の回転角度θ２、及び滑り量Ｓとから算出（推定）する。実トルクτａｃｔは、τａｃｔ＝ｋｒ・（θ２−θ１−Ｓ）、の関係式で求めることができる。このように、被回動部材２２に外力が付与されることで生じる第２プーリ２０の滑り量Ｓを、θ２−θ１から除算することで、モータ１２から被回動部材２２に付与される実トルクτａｃｔを求める（推定する）ことができる。

演算部６２は、トルク値算出部６０が算出した実トルクτａｃｔと後述するトルクτｃとの差分（τａｃｔ−τｃ）を求め、ローパスフィルタ６８は、該差分（τａｃｔ−τｃ）にローパスフィルタ処理を施して、外乱トルクτｄを抽出する。

演算部６４は、モータ１２から被回動部材２２に付与すべき目標トルクτｒｅｆと、外乱トルクτｄとの差分であるトルクτｃ（τｃ＝τｒｅｆ−τｄ）を算出する。トルクτｃは、演算部６２、６６に入力される。

演算部６６は、トルクτｃと実トルクτａｃｔとの差分である差分トルクτｔ（τｔ＝τｃ−τａｃｔ）を求める。ＰＤ制御部７０は、差分トルクτｔに基づいてＰＤ制御（比例微分制御）により、モータ１２の要求トルクτｍｃを算出して、ドライバ７２に出力する。ドライバ７２は、要求トルクτｍｃに基づいてモータ１２を駆動させる。

従って、モータ制御部５２は、滑り量Ｓを用いて、適切にモータ１２から被回動部材２２に付加される実トルクτａｃｔが目標トルクτｒｅｆとなるようにモータ１２を制御することができる。なお、この目標トルクτｒｅｆは、滑り開始トルクＴｓ１未満のトルクに設定してある。

このように、第１プーリ１６の回転角速度ｄθ１の変化量、第１プーリ１６と第２プーリ２０との回転角度差（θ２−θ１）、及びモータ１２の出力トルクτｍに基づいて滑り量Ｓを推定するので、モータ１２から関節である被回動部材２２に加わる実際の実トルクτａｃｔを推定することができ、関節である被回動部材２２の回動を加速度、速度、位置の次元で適切に制御することができる。

また、モータ１２が駆動される前のワイヤ１８の引っ張り力Ｆｐ、ワイヤ１８の全長Ｌ、第２プーリ２０とワイヤ１８との静止摩擦係数μ、第１プーリ１６及び第２プーリ２０間の距離、第１プーリ１６及び第２プーリ２０の半径ｒ、Ｒ、及び駆動連結部材１４の減速比に基づいて、第１プーリ１６がワイヤ１８に対して滑り始める滑り開始トルクＴｓ１が設定され、モータ制御部５２は、滑り開始トルクＴｓ１未満のトルクを出力するようにモータ１２の出力トルクを制御するので、モータ１２の駆動によって第１プーリ１６がワイヤ１８に対して滑ることを防止することができる。

なお、上記実施の形態では、滑り量算出部５０は、数式（４）で求めた滑り量Ｓにローパスフィルタ処理を施すようにしたが、数式（４）で求めた滑り量Ｓにローパスフィルタ処理を施さなくてもよい。その理由としては、滑り量Ｓは、第２プーリ２０のワイヤ１８に対する滑り量が殆どであり、この場合は、第１プーリ１６の回転角速度ｄθ１及びトルクτｒの変化は殆どなく、第１プーリ１６と第２プーリ２０との回転角度差は、主に低周波で変化するからである。

また、上記実施の形態では、第１プーリ１６である第１部材及び第２プーリ２０である第２部材は、回転運動を行うようにしたが、直進運動するものであってもよい。この場合は、モータ１２の回転力を直進力に変換して第１部材に該直進力を伝達させる。この場合であっても、回転運動の場合と同様の効果を得ることができる。

以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に、多様な変更または改良を加えることが可能であることが当業者に明らかである。その様な変更または改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。

１０…駆動装置 １２…モータ
１４…駆動連結部材 １６…第１プーリ
１８…ワイヤ ２０…第２プーリ
２２…被回動部材 ２４…制御部
２６、２８…変位エンコーダ
３０…トルクセンサ ５０…滑り量算出部
５２…モータ制御部 ６０…トルク値算出部
６２、６４、６６…演算部 ６８…ローパスフィルタ
７０…ＰＤ制御部 ７２…ドライバ

Claims (3)

1. 回転駆動源と、
   前記回転駆動源の出力側に連結される駆動連結部材と、
   前記駆動連結部材の出力側に連結される第１部材と、
   関節である被回動部材が連結された第２部材と、
   前記第１部材の回転力を第２部材に伝達する弾性部材と、
   前記第１部材及び前記第２部材の変位量を検出する変位量検出手段と、
   前記第２部材の前記弾性部材に対する滑り量を算出する滑り量算出部と、
   前記滑り量に基づいて前記回転駆動源の出力トルクを制御する回転制御部と、
   を備え、
   前記回転駆動源が駆動される前の前記弾性部材の引っ張り力、前記弾性部材の全長、前記第２部材と前記弾性部材との静止摩擦係数、前記第１部材及び前記第２部材間の距離、前記第１部材及び前記第２部材の半径、及び前記駆動連結部材の１：１を含む減速比に基づいて、前記第１部材が前記弾性部材に対して滑り始める滑り開始トルクが設定されており、
   前記回転制御部は、前記滑り開始トルク未満のトルクを出力するように前記回転駆動源の出力トルクを制御し、
   前記滑り量算出部は、前記第１部材の変位速度の変化量、前記第１部材と前記第２部材との変位量差、及び前記回転駆動源の出力トルクに基づいて前記滑り量を算出する
   ことを特徴とする駆動装置。
2. 請求項１に記載の駆動装置において、
   前記滑り量算出部は、下記の関係式によって、滑り量を算出する
   ことを特徴とする駆動装置。
   Ｓ＝Ｉ１／ｋｒ・（ｄθ１（ｎ＋１）−ｄθ１（ｎ））／ｄｔ＋（θ２（ｎ）−θ１（ｎ））−τｒ（ｎ）／ｋｒ
   （但し、Ｓ：前記滑り量、 Ｉ１：前記第１部材、前記駆動連結部材、及び前記回転駆動源の慣性モーメント、 ｋｒ：前記第１部材及び前記第２部材間のねじりばね定数、 θ１（ｎ）：タイミングｎで検出された第１部材の変位量、 θ２（ｎ）：タイミングｎで検出された第２部材の変位量、ｄθ１（ｎ）：タイミングｎで検出された第１部材の変位速度の変化量、 τｒ（ｎ）：タイミングｎで検出された駆動連結部材の出力トルク）
3. 請求項２に記載の駆動装置において、
   前記滑り量算出部は、算出した滑り量にローパスフィルタ処理を施して該滑り量を補正する
   ことを特徴とする駆動装置。
   

Images