䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣏䢴䢯䢲䢶

並列的解法を用いた２関節劣駆動リンク系のフィードフォワード制御○北村 悠人(筑波大院)

磯部大吾郎(筑波大)Feed-Forward Control of Two-Joint Underactuated Link System
Using Parallel Solution Scheme
Abstract －In this paper, the parallel solution scheme is applied to the calculation of inverse dynamics for an underac-tuated link system with consideration of friction at the passive joint. A solution scheme of kinematics with successive
revision algorithm is applied in order to calculate target trajectories that compensate for the inertial forces acting in a system consisted of passive joints and flexible links. Some results of inverse dynamics calculation and experiments of feedforward control on a two-joint underactuated link system are shown.

Key Words: Inverse dynamics, Underactuated link system, Parallel solution scheme, Feedforward control, Friction

1.

緒言

本研究では，二つの関節のうち一つが駆動関節で，もう一つが受動関節である２関節劣駆動リンク系の制御を行う．劣駆動リンク系とは，システムの自由度に対しアクチュエータの数が少ないシステムである．通常のリンク系ではモータのようなアクチュエータが必要となる関節を受動関節とすることで，系の軽量化・省電力化・アクチュエータが少なくて済
むことによるシステムの簡素化およびコストダウンなど，多くの利点が得られる．さらに，アクチュエータが故障した際に動作を補償する，フェールセー
フ機構としての応用も考えられる．しかしながらこのようなシステムは，アクチュエータにより直接制御できない受動関節が存在するために，当然ながらその制御は困難となる[1][2]．
一方，有限要素法（FEM）に基づいて開発された並列的逆動力学計算法[3]（以下，並列的解法と記す）を用いると，劣駆動リンク系を対象としても正確なトルクが算出可能である[4]．さらに本解法は，柔軟なリンク部材を有する劣駆動リンク系に対しても，その部材剛性に起因する曲げ振動やたわみを考慮した逆動力学計算が可能である[5]．しかしこれらの既存の研究では，受動関節に生じる摩擦を考慮しておらず，また，実際の制御で検証が行われていなかった．そこで本研究では，まず Fig.1 に示す二つのギアレスモータを有する基本的な 2 リンクマニピュレータを検証用の実機として製作した．モータ 1 は駆動関節，モータ 2 は駆動せずに受動関節とし，エンコーダの検知情報のみを使用する．次に，受動関節に発生する摩擦を考慮した軌道を有限要素アプローチに基づいた軌道計算アルゴリズムにより算出し，それを用いて並列的解法によりトルクを算出した．算出結果を実機のフィードフォワード制御に適用した結果，解法の妥当性が確認されたのでここに報告する．

2.

軌道計算アルゴリズム

2-1.変形量の計算
並列的解法は，動的な影響を含んだ節点力を求めれば全関節のトルクが算出されるため，その計算過程は部材剛性に依らず同一のものとなる．したがって，部材剛性や減衰を考慮した正確な軌道を作成することが重要なポイントとなる．以下にFEM を用いた軌道計算アルゴリズムについて記す．
系の移動によって生じる体積力を考慮すると，仮想仕事の原理より時刻 t+⊿t における増分型運動方
程式は次式のように定式化される．

[M]{um}t+∆t +[M]{ud}t+∆t +[C]{ud}t+∆t +[K]{∆ud}= {Fdrag}t+∆t +{F}t+∆t −{R}t(1)ここで，[M]は全体質量マトリクス，[C]は全体減衰マトリクス，[K]は全体剛性マトリクス，{F}は外力ベクトル，｛R｝は内力ベクトル，{um}は系の移動量ベクトル，{ud}は系の変形量ベクトルである．また，{Fdrag}は抵抗力に関するベクトルであり，摩擦抵抗などを表す．今回の研究では関節 2 が受動関節であるため，その摩擦抵抗として，{Fdrag}の成分のうち受動関節の回転方向に関する成分fθ2をMotor 1

Motor 2

Fig.1 Two-joint underactuated link system➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛(=f θ 2 γ θ2 − θ1)(2)

䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣏䢴䢯䢲䢶とする．

t −1

Ｙt

{u d }t3.

並列的解法では，入力データとして与えられる軌
道より陽に算出される節点力を，力学的な関係に基づいてトルクに換算する．節点力-トルク換算式をマトリクス形式で表したものが次式である[3]．

θ t −1θtθ t +1ϕt −1θtInitial trajectory
Input trajectory at each time step
Revised trajectoryＸFig. 2{τ }= [L ][T ]{P }nt +1ϕt
ϕ t +1

並列的解法

Concept of successive trajectory revisionalgorithm for underactuated link systemsとした．ここで， θ1,θ2 は全体座標系での関節 1，2の回転角速度，γは実験から求めた摩擦係数である．
(1)式に初期の目標軌道として時刻毎の{um}を入力すると逐次的に{ud}が算出され，この 2 つの変位量の和として剛性や減衰・摩擦抵抗を考慮した軌道が得られる．時間積分法としては，Newmarkのβ法(δ=1/2,β=1/4)を使用した．
さらに本研究では，(1)式の全体剛性マトリクス[K]の軸方向以外の成分を 0 とすることで、受動関節における屈曲を変形として計算することとした．他
方，受動関節が系の中に存在すると，初期の入力軌道（剛体リンク系のもの）と出力軌道（劣駆動リンク系のもの）との間に大きな差異が生じてしまうため，正確な軌道が算出できない．その対処法として，入力軌道である剛体リンク系の軌道をステップ間で逐次更新する，軌道更新アルゴリズム[4]を使用した．
2-2.軌道更新アルゴリズム[4]軌道更新アルゴリズムについて本稿では，水平面内における 2 リンク系モデルを例として説明する（Fig. 2 参照）．まず，任意の時刻tの入力軌道が既知であるとし，この入力軌道を [K] マトリクスの軸方向以外の成分を 0 とした (1) 式に入力し，2 リンク目に生じる変形量 {ud}t を算出する．次に，リンク先端の変形量から受動関節の変形角θ t を算出し，系の変形角θt を保持したまま，１リンク目の関節を剛体リンク系の入力軌道通りにφt+1へ回転させ，時刻t+1 における入力軌道を決定する．この操作をステップ毎に逐次的に行うことで，劣駆動リンク系の軌道を算出することが可能となった．なお，
0 step目の各節点の初期速度および初期加速度については，初期の剛体リンク系のものを用いること➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛

nnn(3)

ここで， {Pn}は節点力に関するベクトルであり，軌道計算から得られる加速度を用いて求められる．[Tn]は全体座標系からリンクの要素座標系に変換する座標変換マトリクスであり，[Ln]は系の部材長・形態を表す部材長マトリクス，そして{τn}は求めるべき関節トルクベクトルである．各変数をマトリクス形式に分離することにより，系の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となっている．

4.

逆動力学計算例

Fig. 3 に本研究で用いたリンク系の概要を，Table 1にそのパラメータを示す．リンク 1 を水平面内で回転させる際に，モータ１に必要となるトルクを並列的解法により計算する．目標軌道は 0 ~ 2[s]でリンク1 をπ[rad]回転させ，その後はリンク 1 を静止Yさせるものとした．
Mass
軌道計算アルゴリズ
Link2ムを用いて上記の軌道を計算した結果を Fig.
J24 に示す．Fig. 4(a)は，(Motor2)
J1: Active
受動関節の摩擦係数をJ2: Passiveγ=0 とした場合の軌道
Link1である．一方 Fig. 4(b)J1には，受動関節の回転(Motor1)
角速度により異なる摩π[rad] X
擦係数を与えた場合のFig. 3 Outline of two-joint
軌道を示す．本研究でunderactuated link system
使用した受動関節では，Table 1 Parameters of link system
Value
Parameter
Link 1
Sectional area: A [m2]2.4×10-4
3Density: ρ [kg/m ]2.97×103
Young’s modulus: E [GPa]206.0
Moment of inertia: I [m4]3.20×10-8
Length: L [m]Head mass: m [kg]0.1420.480(Motor 2’s weight)

Link 20.163
0.00

䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣏䢴䢯䢲䢶

0.2

0.2

J2J1

J1

Y[m]

0-0.2-0.2

0X[m]

00.2

X[m]

0.2

(a)Without friction(b)With friction
Fig. 4 Calculated trajectories for two-joint
underactuated link system

J1(Active)
J2(Passive)

0.05
0-0.05J1(Active)
J2(Passive)

0.04
Torque[Nm]Torque[Nm]

Y[m]

J20

Table 2 Parameters of acryl link
ValueParameter
Link 1
Link 22-4
Sectional area: A [m ]0.4×10Density: ρ [kg/m3]
1.19×103
Young’s modulus: E [GPa]3.2
4
Moment of inertia: I [m ]1.3333×10-11

00.02

123Time[s]45

Fig. 6 Calculated torque considering elastic
deformation of acryl member

0系の固有振動であり，部材の弾性たわみを考慮したために発生した弾性振動に起因するものである．

-0.02
-0.04
012

3Time[s]

455.

制御実験

Fig.1 に示した実機を用い，制御実験を行った．目
標軌道には前節に示した軌道を用い，制御法としては逆動力学計算によって求めたトルク(Fig. 5)の入力
1.2[rad/s]以下の回転角速度ではγ=0.00013，それ以上のみによるフィードフォワード(FF)制御，J1 の角度の回転角速度ではγ=0.000025 とした．いずれの場合情報を使ったフィードバック(FB)制御，そして両者も，リンク 1 の停止後にも先端に残る加速度によりを組み合わせた FF+FB 制御の 3 通りを行い比較する．
リンク 2 が移動していることが分かる．
また，Fig. 4(b)Fig. 7 にこれらの実験のそれぞれの関節角度結果をを見ると，摩擦を考慮することで受動関節が回転し示す．
にくくなり，結果的にリンク先端が大きく動いていFig. 7(a)の FF 制御では，J2 については途中の 2~3[s]ることが分かる．また，5 [s]の時点ではほぼ静止してでシミュレーションによる角度と実験結果に違いがいる結果となった．
見られるが，最終角度は良好に一致しているといえこの軌道を並列的解法に入力した結果，得られたる．また，2 [s]周辺で J1 の角度が目標角度より少しトルク曲線を Fig. 5 に示す．この曲線は Fig. 4(b)を入小さくなったが，これは外乱の影響を多少受けたた力して計算したもの，すなわち摩擦を考慮した軌道めであると考えられる．一方，Fig. 7(c)では FB 制御についてのトルク曲線である．なお，解析は時間増を組み合わせているため，J1 の角度追従性が向上し分⊿t=0.01 [s]で計算した．Fig. 5 を見ると，関節 J2ている．J2 の回転角は FF 制御の場合と大きくは変わのトルクが 0 を維持していることが分かる．これは，らないが，最終角度が目標角度に近くなった．Fig.
算出された軌道を用いると J2 にトルクを必要としな7(b)の FB 制御では J1 の立ち上がりに遅れが発生していことを示しており，劣駆動関節を正確に表現できいることが分かる．この影響で J2 の回転量が小さくているものと言える．また，Fig. 5 に見られるトルクなっている．また，リンク 2 の動きを考慮できてい曲線上の微小振動は，部材であるステンレスの剛性ないため，2 [s]で静止させる時にリンク 1 がリンク 2を考慮した結果である．ステンレスのような硬い部に引っ張られる形になり，J1 の角度にオーバーシュ材を用いるとこのような高周波の振動が発生するが， ートが見られる．Fig. 8 には，実際に制御実験で使用リンク 1 として仮に Table 2 に示すような柔軟部材でしたトルク曲線を示す．FF 制御時と FF+FB 制御時のあるアクリルを用いた場合に同様の解析を行なうと， トルク曲線がほぼ同様なものとなった．これらの結そのトルクは Fig. 6 のように計算される．以上のよ果から，並列的解法によって計算されたトルクが妥
うに，J1 のトルク曲線に出現する振動はこのリンク当なものであると言える．
Fig. 5 Calculated torque➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛䣔䣕䣌䢴䢲䢲䢻䣃䣅䢵䣏䢴䢯䢲䢶

0.06

20.02(a) FF

1
J1 Target
J1 Result
J2 Target
J2 Result00123

4-0.04

5Tme[s]-0.060Angle[rad]3123Time[s]45Fig. 8 Control torque2(b) FB1

参考文献

J1 Target
J1 Result
J2 Target
J2 Result0-1012345Time[s]3Angle[rad]0-0.02-12(c) FF+FB

1J1 Target
J1 Result
J2 Target
J2 Result0-1012345Time[s]Fig. 7 Control results

6.

FF
FBFF+FB

0.04

Torque[Nm]Angle[rad]3

結言

本研究では，並列的解法を用いて劣駆動リンク系の逆動力学計算を行った．計算されたトルクを用いて実機でフィードフォワード制御を行った結果，算出されたトルクの妥当性とフィードフォワード制御の有効性を確認した．また，受動関節に発生する摩擦抵抗を考慮することにより，シミュレーションされた軌道と実機での軌道の良好な一致を確認した．
このような受動関節，摩擦抵抗を考慮した計算を容易に実行できるのは FEM に基づいた並列的解法の特徴である．今後は，摩擦係数の決定を定式化すること，さらに先端リンクの位置制御や多関節劣駆動リンク系の制御への適用を考えている．

➨䢴䢹ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢻ᖺ䢻᭶䢳䢷᪥ࠥ䢳䢹᪥凛[1] 鈴木高宏，三好渉，中村仁彦：”非ホロノミック自由関節マニピュレータの制御”,日本ロボット学会誌,
19(4), pp.499-509, 2001.
[2] 荒井裕彦：”非駆動関節を有する 3 自由度マニピュレータの非ホロノミック拘束下における可制御性”,日本ロボット学会誌, 14(5), pp.751-758, 1996.
[3] D. Isobe: “A Unified Solution Scheme for Inverse Dynamics”, Advanced Robotics, 18(9), pp.859-880, 2004.
[4] 北村悠人，山中浩司，磯部大吾郎：”並列的解法による劣駆動リンク系の逆動力学計算”，ロボティクス・メカトロニクス講演会’08 講演論文集.
[5] 北村 悠人，磯部大吾郎：”弾性たわみを考慮した劣駆動リンク系の逆動力学計算”, 第 26 回日本ロボット学会学術講演会論文集.
[6] 磯部大吾郎，加藤 昭博：”フレキシブル・リンク系のモデルベースド・フィードフォワード制御”，日本
ロボット学会誌，25(4), pp.625-631, 207.