4 892pulse/rot 0Hz 50Hz 2 00Hz 300Hz 2 CPU PID 6) 4, 7) 8) , 2) 2 M M θm + D M θm + f M sgn( θ M ) = τ N G [K G (N G θ M θ L ) + D G (N G θm

Size: px

Start display at page:

Download "4 892pulse/rot 0Hz 50Hz 2 00Hz 300Hz 2 CPU PID 6) 4, 7) 8) , 2) 2 M M θm + D M θm + f M sgn( θ M ) = τ N G [K G (N G θ M θ L ) + D G (N G θm"

さゆりこしの
5 years ago
Views:

65-4-2 シリアル 2 リンク 2 慣性系の同定と状態フィードバック制御大明準治株東芝研究開発センター足立修一慶應義塾大学 Identiﬁcation and State Feedback Control of Serial Two-link-two-inertia System Junji OAKI (Toshiba Corporate R & D Center) and

1 シリアル 2 リンク 2 慣性系の同定と状態フィードバック制御大明準治株東芝研究開発センター足立修一慶應義塾大学 Identiﬁcation and State Feedback Control of Serial Two-link-two-inertia System Junji OAKI (Toshiba Corporate R & D Center) and Shuichi ADACHI (Keio University) Abstract In this paper, we propose an identiﬁcation method for a SCARA-type planer two link robot arm with elastic reduction gears, which is treated as a serial two-link-two-inertia system. In the identiﬁcation method, we utilize not only motor encoders, but also rate gyros and accelerometers on each link of the arm. The identiﬁcation method consists of three steps. The ﬁrst step is the rigid model identiﬁcation using a well-known procedure. The second is the elastic model identiﬁcation using a multi-input multi-output state space model technique in which the interaction forces between two links are decoupled. The third is the physical parameter identiﬁcation using the state space model. We also show experimental results of full state feedback control schemes including low pass ﬁlters designed by using the identiﬁed model parameters. Key Words: Robot arm, Rate gyro, Accelerometer, Mechanical resonance, Elastic joint, Harmonic drive はじめに精密組立やマテリアルハンドリングなどで用いられる産業用 SCARA スカラ型ロボットは水平旋回する第 2 関節の減速機の弾性に起因した振動を抑制しながら高速高精度に動作することが要求される従来のモーションコントロールにおける振動抑制は 2 多慣性系の制御問題として扱われてきたその基本的な考え方は入出力系外乱抑制制御でありスカラ型ロボットのようにシリアルに 2 慣性系が連なった非線形の多入出力連成振動系には効果的な制御方法とは言えなかった弾性関節を持つロボットに対して効果的と考えられる例としては西田ら ) や島田ら 2, 3) による各軸を駆動するモータに内蔵されたエンコーダだけの情報を用いた状態空間表現での外乱オブザーバベースの巧妙な制御方法が挙げられるこれらは現状のセミクローズドループ制御方式のスカラ型ロボットに適用でき魅力的ではあるが制御系の実装は複雑であり関節弾性まで考慮に入れたモデル化のための物理パラメータ同定も容易ではないまたロボットのダイナミクスを遅い剛体モードと速い振動モードに分けて考える特異摂動法に基づくアプローチ 4, 5) についても同様な問題を抱えている本稿ではモータ内蔵エンコーダに加えて近年入手容易になったレートジャイロや加速度センサをスカラ型ロボットの各リンクに搭載することを前提としてシリアル 2 リンク 2 慣性系の干渉構造を利用した多入出力状態空間同定法を提案し水平旋回型 2 リンクアームの実機で検証する提案法では第関節と第 2 関節を非干渉化し 2 つのリンク 2 慣性系の同定問題に帰着させているところがポイントであるさらにこの同定結果を用いてシリアル 2 リンク 2 慣性系の運動方程式モデル 8 次をシミュレーションするのに便利なように各関節の慣性や摩擦バネ定数を求める方法にも言及するこれは弾性関節を持つロボットの時系列データから剛体モデルの物理パラメータだけでなく弾性モデルの物理パラメータまで同定しようという試みであるが従来ほとんど手がつけられていなかった研究課題である第7回制御部門大会 2007年3月6日 8日調布リンク搭載センサを利用すればシリアル 2 リンク 2 慣性系モデルの範囲では全状態フィードバック制御が可能になるしかしリンク搭載センサが検出する高次成分に起因するスピルオーバが問題となるそこで本稿では簡易なロバスト制御方法であるローパスフィルタ包含型状態フィードバック制御則について検討する最後に状態フィードバックを用いてロボットを軌道制御する際に偏差を少なくするために必要なリンク角速度目標値軌道を時間区分多項式ベースで生成したモータ角速度目標値軌道から変換するための目標値フィルタの同定についても述べる 2 対象とするシステム本稿で制御対象とするのは Fig. に示すような水平旋回型の 2 リンクアーム各リンクの長さ m である各関節を駆動する DC モータ第軸 500 W 第 2 軸 300 W は各リンク上に配置されバネ要素として振る舞うハーモニックドライブ減速機減速比 /50 が直結されているこれは平行リンクを用いてないスカラ型ロボットの第 2 関節を模擬しておりリンク 2 慣性系が直列に 2 つ配置されているのでシリアル 2 リンク 2 慣性系と呼ぶことにする第 2 関節の駆動系は全く同じ構造をしているが第 2 関節は並進運動をするので並進運動しない第関節とは異なる複雑な振動特性を示すのが特徴である 2nd Encoder st Rate Gyro 2nd Motor st Accelerometer 2nd Rate Gyro st Motor,Encoder, Harmonic Drive 2nd Harmonic Drive 2nd Accelerometer Fig. : Planer two link arm with elastic joints SY0003/07/ (c) 2007 SICE

2 4 892pulse/rot 0Hz 50Hz 2 00Hz 300Hz 2 CPU PID 6) 4, 7) 8) , 2) 2 M M θm + D M θm + f M sgn( θ M ) = τ N G [K G (N G θ M θ L ) + D G (N G θm θ L )] () M L (θ L ) θ L + c L ( θ L, θ L ) + D L θl = K G (N G θ M θ L ) + D G (N G θm θ L ) (2) θ M = [θ M, θ M2 ] T 2 θ L = [θ L, θ L2 ] T M L (θ L ) R 2 2 c L ( θ L, θ L ) R 2 M M = diag(m M, m M2 ) D M = diag(d M, d M2 ) D L = diag(d L, d L2 ) K G = diag(k G, k G2 ) D G = diag(d G, d G2 ) N G = diag(n G, n G2 )n G, n G2 f M = [f M, f M2 ] T τ = [τ, τ 2 ] T α, β, γ 9) [ ] α + β + 2γ cos(θl2 ) β + γ cos(θ M L (θ L ) = L2 ) β + γ cos(θ L2 ) β (3) [ c L ( θ γ(2 θl θl2 + L, θ L ) = θ L2 2 ) sin(θ ] L2) γ θ L 2 sin(θ (4) L2) cos(θ L2 ) sin(θ L2 ) θ L2 = n G2 θ M2 ()(2) β, γ () (2) θ M = N G θ L M(θ L ) θ L + c L ( θ L, θ L ) + D θ L + f M sgn( θ M )/N G = τ /N G (5) 9) M(θ L ), D (6)(7) M(θ L ) = [ ] α + β + 2γ cos(θl2 ) + m M /n 2 G β + γ cos(θ L2 ) β + γ cos(θ L2 ) β + m M2 /n 2 G2 (6) D = diag(d L + d M /n 2 G, d L2 + d M2 /n 2 G2) (7) 2) ω ω 2 θ L = ω (8) θ L2 = ω 2 ω (9) 2 a a 2 d d 2 θ L = a (0) d θ L2 = a 2 a a cos(n G2 θ M2 ) d 2 d d 2 d d 2 θ2 L sin(n G2 θ M2 ) () 0) 0Hz 50Hz 0Hz 2

3 G L (s) G H (s) = G L (s) T V θ Li(wide) = + T V s θ Li + T V s + T V s θ Li s = + T V s ( θ Li + T V θli ) (i =, 2) (2) (2) θ Li(wide) θ Li T P θ Li + T P s n Gθ Mi + T P s + T P s θ Li s = + T P s (n Giθ Mi + T F θli ) (i =, 2) (3) (5) 2 9) β, γ 5..2 ()(2) ()(2) ẋ = Ax + B (β + γ cos(θ L2 )) θ L2 (4) γ(2 θ L θl2 + θ L2 2 ) sin(θ L2) y = [ θ M θl ] T = Cx (5) τ (3) (,) (5) 6 ()(2) (4)(5) θ M (s) =G (s) τ + G 2 (s) (β + γ cos(θ L2 )) θ L2 G 3 (s) γ(2 θ L θl2 + θ 2 L2) sin(θ L2 ) (6) G (s) 2 2/3 ) G (s) = b 0 + b s + b 2 s 2 a 0 + a s + a 2 s 2 + a 3 s 3 (7) a 0 = d M + n 2 Gd L a = m M + n 2 Gm L + (n 2 Gd G d L + d M d L + d M d G )/k G a 2 = (m M d L + m M d G + m L d M + n 2 Gm L d G )/k G a 3 = m M m L /k G b 0 = b = (d L + d G )/k G b 2 = m L /k G (3) (,) m L = α + β + 2γ k G (7) 6 (4)(5) 2 G 2 (s) / Fig. M msM ms Hz d = 8 2ms 50Hz 00Hz 5Hz Fig. 2 M 0.3 Fig. 3 2 Fig. 4 Fig. 5 2

4 Fig. 2: M sequence input and its decimated data for st link identification (First 0.3s of 2.046s) cos (4) MATLAB TM pem 2) 3 pem (n4sid) Fig. 6 θ M 2/3 θ L /3 2 M Fig. 7 ()(2) SIMULINK TM 2 2 PID 3) Fig. FF-I-P Fig. 9, 0 pem 0 Fig. 6 2 Fig (3) (2) 2 2 Fig. 3: 2nd link acceleration data for st link identification Fig. 4: Motor and link velocity data for st link identification β, γ 4) ) Fig. Fig. 5: st link angle and cosine of 2nd link angle data during st link identification

5 (a) τ θ M (a) τ 2 θ M2 (b) τ θ L Fig. 6: Identified frequency responses (st link) 0.25ms 5.4 3) G R (s) 2 G R2 (s) (4)(5) θ M G (s)2/3 θ L G 2 (s)/3 G R (s) = G 2 (s)/g (s) (8) /2 G R2 (s) ) (b) τ 2 θ L2 Fig. 7: Identified frequency responses (2nd link) Fig. 8: Velocity step responses comparison (st link) Left: Real arm, Right: Simulation ) 2, 2-3, 466/47(994) 2), 4-5,

6 (a) τ θ M (a) τ 2 θ M2 (b) τ θ L Fig. 9: Identified frequency responses (st link) Single input case 466/472(2005) 3), 42-2, 39/46(2006) 4) M.C.Readman: Flexible Joint Robots, CRC Press (994) 5) C.C.de Wit, B.Siciliano and G.Bastin (Eds): Theory of Robot Control, 79/27, Springer (996) 6) G.Hirzinger, N.Sporer, A.Albu-Schäffer, M.Hähnle, R.Krenn, A.Pascucci and M.Schedl: DLR s torquecontrolled light weight robot III are we reaching the technological limits now?, Proc IEEE International Conference on Robotics and Automation, 70/76(2002) 7), , 944/95(99) 8), 23-2, 49/55(2003) 9) 62/80, (997) 0), 238/239, (989) ), 84/85, (999) 2) L.Ljung: System Identification Toolbox For Use with MATLAB (Version 6.2), The MathWorks(2006) (b) τ 2 θ L2 Fig. 0: Identified frequency responses (2nd link) Single input case Fig. : Velocity step responses comparison (st link) Left: State feedback, Right: State feedback with filter 3), 9-, 55/64(99) 4), 2/5, (2006) 5), 08/23, (2003) 6) MATLAB, 26/25, (2003)

SICE東北支部研究集会資料(2012年)

SICE東北支部研究集会資料(2012年) 77 (..3) 77- A study on disturbance compensation control of a wheeled inverted pendulum robot during arm manipulation using Extended State Observer Luis Canete Takuma Sato, Kenta Nagano,Luis Canete,Takayuki