3D14

並列的逆動力学計算法によるリンク機構の力制御○佐藤俊介(筑波大院)

磯部大吾郎(筑波大)Force Control of Link Systems by Using a Parallel Solution Scheme*Shunsuke SATO, Graduate School,Univ.of Tsukuba, Daigoro ISOBE, Univ.of Tsukuba
Abstract －In this paper, a parallel solution scheme is applied to a feed-forward force control of link mechanisms.
This scheme uses the inverse solution algorithm of the Finite Element Method (FEM), and calculates the joint torqueby using the relationship between nodal force and torque. By making use of this feature, we applied the scheme into afeed-forward force control. In conventional schemes, we need to calculate both inverse dynamics and the torque by
multiplying Jacobian matrix to generating force, to handle such cases. In contrast, the parallel solution scheme canobtain both values in the same algorithm, and in the same process. In this paper, some numerical tests are carried out,
along with some feed-forward force control experiments in the motion much affected by dynamics.
Key Words: Force Control, Feed-forward Control, Link Systems, Inverse Dynamics, Parallel Solution Scheme

1. はじめに

2. 並列的逆動力学計算アルゴリズム

近年のロボット技術の発達に伴い，さまざまな機
Fig.1 のようにモデル化されたリンク機構に対し，構をもつロボットが開発され，ロボットに求められ目標軌道に追従するために必要な関節トルクを算出るタスクも非常に多様化，高速化されてきている．
する逆動力学計算アルゴリズムについて説明する．
ロボットに高速かつ複雑な動作をさせる際，遠心力
まず，仮想仕事の原理より時刻t+Δtにおける運動方やコリオリ力といった非線形力項の影響は無視でき
程式は以下のように定式化される．
ないものとなってくる．これらの項を補償するために逆動力学計算を行い，算出されたトルク値をフィ[M]{u&&}t+∆t +[C]{u&}t+∆t +[K] ∆u ={F}t+∆t −{R}t −{FE} (1)ードフォワード的に入力ことが有効であることが知られている．一方，我々が普段何気なく行っている
ここで[Ｍ]は集中質量マトリクス，[Ｃ]は減衰マト
動作のうち，例えば手先の力を感知し，微妙な力加リクス，[Ｋ]は剛性マトリクス，{u}は節点変位ベク減の制御を行うような場合がある．今後，人間のよトルである．また， {F }は外力ベクトル， {R}は要素うな動作，人間と協調して作業を行うといった，よ内力ベクトル， {FE }は発生力ベクトルである．本稿
り親和性の高いロボットを製作しようとした際には，では，剛体リンク系を扱うため剛性項と減衰項を省力制御を行う必要性がより高まるものと考えられる．
略する．
現在，一般的にリンク系の動力学方程式は，ニュFig.1は，ｎリンク機構においてi番目のリンクがートン・オイラー法やラグランジュ法などにより導必要とするトルク，およびそれをリンクに作用する出されている．これらの手法は，開ループ系の場合節点力によって表した図である．入力された発生力は簡単に厳密な方程式が得られるが，系内に閉ルーベクトル {FE }は，作用する節点の各節点力成分に含プ系が形成された途端に，その導出が困難になってまれている．i番目のリンクに必要となる関節トルクしまうという問題が存在する．そこで筆者の一人は，は，ｉ番目のリンクの重心に働く並進力に起因するリンク機構の形態に依らない統一的解法を開発することを目的とし，有限要素法(FEM)を用いた並列的逆動力学計算法を提案した1)．この計算法は，系全体をτ (i +1) y∑F∑F
有限要素で離散化し，並列的に節点力を求め，力学FiZFiφY的関係よりトルクを算出するという特徴を持っていτ (i +1) z∑F
FiφZFiYる．本研究では，この節点力を操作することで，リτ (i +1) x
τ iyZ
FiX
ンク先端に発生させる力を表現する．既存の手法で
FiφX
τ iz
は，動力学を考慮したトルクと，力を発生させるために必要となるトルクを分けて考えなければならな
τ ixYかった．それに対し本解法では，トルク計算を同一アルゴリズム上で同時に行うことが可能となる．
X
本報告では，既存の手法とのシミュレーションに: Nodal point: Nodal point expressing center of gravity
よる比較を行い，並列的逆動力学計算法を組み込ん(a) Joint torque(b)Nodal Force
だ制御系による，動力学の影響の大きい条件下での力制御実験の結果について報告する．
Fig.1 Joint torque and nodal force acting oni-th link in n-link mechanism{ }(i +1) CZ(i +1)CY( i +1)CX

第21回日本ロボット学会学術講演会（2003年9月20日〜22日）

し，トルク値を計算する．
回転モーメント，リンクの先端に働く並進力，つまり i+１～ｎ番目までのリンクに働く並進力の和に起3. 数値例
因する回転モーメント，および重心回りに作用する慣性モーメント，さらにこれらの和に i+１番目のリ
本研究では，並列的逆動力学計算法を用い， フィンクの関節トルクを加算したものとなる．ｉ番目のードフォワード的にリンク機構の力制御を行う．既リンクにおける関節トルクを節点力によって表すと，存の手法で同等のことを実施する場合，動力学を考次式のようになる．
慮したトルクとしてニュートン・オイラー法等から導出された動力学方程式を解き，発生させたい力に n (2)τ ix = liC Fiy + li  ∑ Fj  + Fiφx + τ (i +1) x
対するトルクとして，ヤコビアンによって力をトル j =i +1  yク値に変換したものを足し合わせる必要がある．これに対し本解法は，力の次元で求解した節点力からトルクを算出する．そのため，動力学を考慮したト
ここで liC は関節から重心までの距離， li はリンクのルクと力を発生させるために必要なトルクを，同一長さ，関節トルクおよび節点力の右下の添字 x, y なアルゴリズム上で同時に算出することが可能である．
どは要素座標系の各軸方向成分であることを示す．
また，従来の手法では，機構の形態が変化した場合
なお，Fig.1 中の大文字の添字は全体座標系の各軸方に動力学方程式，ヤコビアン共に再導出しなくては向成分であることを示す．
ならないのに対し，本解法ではマトリクス形式の運同様に y, z 軸周りの計算式を導出し，さらに動方程式を解くため，入力データの変更のみで対応
i=1・・・,n についてマトリクス形式に直して全体座が可能という利点も含まれている．
標系で整理すると，
θ[rad]{τ }= [L ][T ]{P }n3 n×1nnn(3)

3 n×9 n 9 n×9 n 9 n×1と表現できる．ここで {τ n } は求めるべき関節トルクベ
クトル，{P n }は節点力に関するベクトルである．また，[ T n ]は全体座標系からリンクの要素座標系に変換する座標変換マトリクスで，対角に 9ｎ個の成分が並ぶ．
さらに，[ Ln ]はリンクの重心までの距離などの情報を含む部材長マトリクスである．
本解法は，(2)式に表されるような節点力とトルクの力学的関係を用いてトルクを算出している．そのため，(1)式に含まれている発生力ベクトル {FE }を操作することにより，リンクに作用させる力に応じたトルクを計算することが可能となる．そして，(3)式に表されるマトリクス形式の方程式を逐次的に計算

RollerZ

θ２

θ1
θ200.2

0.4

0.6

0.8

Fig.3 Target (rotational angle)

1 T[s]Torque[Nm]
0.15

ｚFE

0.05

Joint2

Link2

-1

0.1

FEZ0

θ１

Link1
Joint1
X

Proposed scheme
Inverse Dynamics[J]T{F}
Conventional scheme0Xθ １ θ ２ ： Rotational angle of Link1,Link2

F E：Generating Force

Fig.2 Case model for simulation

00.2

0.4

0.60.8

Fig.4 Torque curves

1T[s]目標軌道が入力された後，並列的解法により逆動力既存の手法との比較を行うために，Fig.2 に示され学計算が行われ，逐次的に全関節のトルク値τI.D.がるようなモデルで壁押し動作をさせる際の解析を行った．目標軌道は，手先の座標位置より逆運動学的算出される．その際には，サンプリング周期ごとにに作成した．この際，各種パラメータはこの後行う
その時間分の計算が増分的に行われ，計算終了後に
実 験 用 モ デ ル に 基 づ き 設 定 を 行 っ た ． Link 1各関節のトルク値が出力される．本実験の場合には(0.142[m],0.10224[kg]), Link 2 (0.180[m],0.1296[kg])，
サンプリング周期を 10 [ms]としたが，この時間分のともに中央を重心位置とし，Joint 2 の位置にモータトルク値は約 0.5 [ms]と十分に短い時間で算出され質量として 0.534[kg], Link 2 の先端にローラおよびセることを確認した．次に，モータに発生する摩擦力ンサの質量として 0.152[kg]を加えた．軌道の回転角の影響やフィードバック値などを加え，最終的な制を Fig.3 のように与え，全体座標系のＸ方向に 0.4[N]御トルクτとする．
位置フィードバック(FBp)値は，前ステップの直交の一定の力で押しながら進むものとした．
座標系における目標角度を要素座標系に座標変換しFig.4 にシミュレーションによる Joint 1 のトルク曲た値と，計測された角度情報により次式のように求線を示す．既存の手法である，ヤコビアンと逆動力学によって算出されたトルクを足し合わせたものが， めた．
本解法によって算出されたトルク値と一致しているτfeedback= Ku(qd −q) +Ki (qd −q) +Kv(q&d −q&) (4)ことが確認できる．以上の結果より，本解法ではこれらを同時に考慮したトルクを算出可能であること
また，力フィードバック（FBｆ）値はが確認できた．

∑Ffeedback= KF(FE −FS)

4. フィードフォワード力制御実験
力制御の方式として，インピーダンス制御，位置と力のハイブリッド制御等が現在までに使用されている．これらの方式の実用化に向けての問題として，大量の計算が必要であること，力学モデルのパラメータが不確かであること，精度の良い力覚センサが無いこと，ユーザが位置と力の制御手順を記述することが困難であることが挙げられる．そこで本研究では，Fig.5 に示すような２種類のフィードバック(FBp,, FBｆ)とフィードフォワード (FF) を組み合わせた２自由度制御系を設計した．このような制御系は，フィードバック制御のみでの負担を軽減すること，非線形力補償，システムのモデル化誤差の補償，負荷補償などに有効であることが知られている 2)．また，生体の小脳を含めた運動系に類似しているとの報告 3)もあり，生体が行うような非常に高度な制御が可能となり得る．
構築した制御系では，PC 上で直交座標系に基づく
τ I.DPC

X , Z, qd , FE

Calc. of Inverse
Dynamics by FEM

Correction

KF+Ffeedback

q&d , qd

FSV

ControlPID&qq,AD Board

X , Z, Qd : Coordinates in global Cartesian coordinates
FE , FS : Target force,actual force
qd , q&d : Target angle and angular velocity: Actual angle and angular velocity
q, q&KF

Control

Torque[Nm]Torque[Nm]devicefunctionfriction－として，逆動力学計算部に入力される．ここで q d ，
q& d はモータの目標角度および目標角速度， q ， q& はエンコーダより取得した実際の角度および角速度，
FE は目標発生力，FSは力覚センサより取得した力の値， Ku ，Ki ，Kv，KF はそれぞれ角度，角度の積分値，角速度,力に対するフィードバックゲインである．
この制御系を用い,フィードフォワード力制御実験を行った.制御対象は，シミュレーションの際と同じFig.2に示す平面2リンク機構とした．モータにはmaxon社製コアレスモータ（品番：148877）を使用し，動力学の影響を大きくするためギア比を1:1とした．
リンク２の先端部に力覚センサおよびローラを取り付けた．リンク部材はジュラルミン製で，それぞれ長さは0.142 [m]，0.180[m]，質量はそれぞれ0.10224[kg]，0.1296[kg]，断面幅は4.0 [cm]，厚みは0.6 [cm]である．モータ単体の質量は0.534 [kg]である．先端の力覚センサとしては,BL.AUTOTEC社製Micro 5/50を使用した．センサ部の重量は0.152[kg]である．目
標軌道として，前章のFig.3に示される軌道を１秒と５秒の場合で用意した．目標発生力を正弦波で与え,LINKτconsideringFE(5): Feedback gain for force

Fig.5 Outline of control system

Encoder

Output TorqueDynamics

0.15

0.1

0.1

Force sensor
0.05

0.05

0Output TorqueDynamics

0012345(a) Torque curves in 5-sec motion T[s]

00.2

0.4

0.6

0.8(b) Torque curves in 1-sec motion

Fig.6 Rate of dynamics in torque curves

1T[s]

F[N]

F[N]

11

θ[rad]
Link1
0

0.5

0.5

Link2
-1

TargetFF+FBp001234(a) Generating force (FF+FBp)

05 T[s]TargetFF+FBpFF+FBp+FBfTarget
FF+FBp+FBf
01234(b) Generating force (FF+FBp+FBf)

05 T[s]1234(c) Rotational angle

5T[s]Fig.7 Control result in 5- sec motion
F[N]
0.8

F[N]

0.60.6

θ[rad]

0.8

Link1
00.4

0.4

Link2

0.2
-10.2

TargetFF+FBpFF+FBp+FBf

0TargetFF+FBp000.2

0.4

0.60.8(a) Generating force (FF+FBp)

1-0.2
T[s]

TargetFF+FBp+FBf
00.2

0.40.60.8(b) Gererating force (FF+FBp+FBf)

1T[s]00.20.4

0.60.8(c) Rotational angle

1T[s}Fig.8 Control result in 1-sec motion
追従性や時間遅れなどを調べた.この動作をさせる
際に必要となるトルク，およびその中に含まれる動力学を考慮したトルク値をFig.6に示す.Fig.6 (a),(b)を比較すると,動作時間１秒の場合はトルク曲線において動力学が大きな割合いを占めることがわかる.
動力学を考慮したトルク値の影響を調べるため，動作時間５秒，１秒で実験を行った．また，(FF+FBp)の場合と(FF+FBp+FBｆ)で実験を行い，フィードフォワード項および力フィードバック項の有効性を調べた．Fig.7，Fig.8に動作時間５秒と１秒の場合の結果を示す．どちらの動作の場合も力のフィードバッ
ク（FBｆ）をかけなくても良好に目標発生力に追従していることが確認できる．また, (FBｆ)をかけた場合，若干追従性が向上していることも確認できた．
位置に関する追従性も(FF+FBp), (FF+FBp+FBｆ)の場合共に良好に追従しており，本実験では，位置，力に関して高い次元で両立していると言える．以上の実験結果から，本研究で設計した逆動力学モデルを組み込んだ２自由度制御系は，力制御を行う際に有効であることが実証された．また，今回の実験のよ
うにセンサからの計測ノイズの影響が大きい場合などには，本制御系はフィードバック制御の負担を軽減させるため，非常に有効であると言える．

5. まとめ既存の手法では動力学を考慮したトルクと，力を発生させるために必要となるトルクを分けて考えなければならなかった．これに対し，本研究で使用した並列的逆動力学計算法は，トルク算出を節点力という力の次元で取り扱うため，同一アルゴリズム上で同時に解くことが可能となった．本報告では，並列的逆動力学計算法を組み込んだ２自由度制御系で簡単な壁押し実験を行い，その実用性を確認した．
今後は，動的な影響の大きな複数リンクでの協調作業に適用する予定である．
参考文献
1) 磯部大吾郎：有限要素法を用いたリンク機構の逆動力学計算，日本ロボット学会誌 Vol.20, No.6,pp. 647-653,(2002).
2) 竹宮隆、石河利寛 編：運動適応の科学,杏林出版,
pp.26-49, (1998).
3) 川人光男：脳の運動学習, 日本ロボット学会誌 Vol.13,
No.1, pp.11-19, (1995).