並列的解法によるフレキシブルマニピュレータの逆動力学計算○加藤 昭博（筑波大院） 磯部 大吾郎（筑波大）
Calculating Inverse Dynamics of Flexible Manipulators by Using Parallel Solution Scheme*Akihiro KATO, Graduate school, Univ. of Tsukuba,

Daigoro ISOBE, Univ. of Tsukuba

Abstract －In this paper, some verifications are done on the calculation cost of a formerly proposedparallel solution scheme for calculating inverse dynamics of flexible manipulators. The solution
scheme, with proper target trajectories as input data, can easily calculate inverse dynamics regardless ofmember stiffness of the constituted links. Target trajectories are calculated by using another scheme,
which is based upon the principle of virtual work, where member stiffness and damping effects are incorporated in the trajectory data. Link members are modeled and compared, between the linear Timoshenko beam and the cubic beam elements. Some numerical tests and experiments are carried out ona simple flexible link system, and the results show that the parallel solution scheme can achieve highaccuracy at low calculation cost.
Key Words: Flexible manipulators, Parallel solution scheme, Inverse dynamics, Trajectory1.

緒言

近年，エネルギ消費量の削減やロボットの製作コスト削減を目的として，ロボット機構のさらなる軽量化の必要性が高まっている．しかし，部材の軽量化はリンク剛性の低下を招き，機構内に発生する弾性たわみや曲げ振動が無視できなくなる．この問題は，機構を完全剛体と仮定して設計することが多い
現状において，その制御を非常に困難なものとする要因の一つとなっている．そのため，弾性たわみを考慮したロボットアームのモデル化[1][2]や逆動力学計算[3]などの研究も精力的に行われている．しかし，従来の動力学方程式には機構の構造や運動などに関する全ての変数が混在し，さらにトルクの次元で求解を行っていた．そのため，弾性たわみの影響を考慮した逆動力学計算には大変複雑な処理を要していた．
他方，連続体力学に基づく数値解析手法として広く利用される有限要素法は，全体直交座標系に基づいて系全体を微小要素に離散化し，全体方程式にまとめて解を求める解析手法である．そのため，各要素の情報が並列的に取り扱われ，系の構造に依存せず，マトリクス計算によって節点力や変位・ひずみ等が算出される．この特長を利用し，リンクの形態に依存しない逆動力学計算法として並列的解法が開発された[4]．従来の手法の一つ，例えばニュートン・オイラー法が相対回転座標系に基づいて再帰的な処理により動力学方程式を求める直列的なアプローチをとるのに対し，本手法は並列的なアプローチをとると言える．本解法では，リンク機構に与えられる目標軌道から，動的な効果を含めた各節点力を算出し，力学的な関係に基づいて各関節トルクに換算する．また，従来の動力学方程式が全ての成分が混在した形をとるのに対し，本解法のトルク換算式は，力の次元に関する成分，座標変換に関する成分，系の構成部材の長さに関する成分ごとのマトリクスに分離される．そのため，機構構造が複雑な場合や制御の途中で構造が変化する場合にも，入力データの変更のみで柔軟に対応できる．これによって，従来の逆動力学計算法では困難であった閉リンク機構や，開・閉ループが交互かつ連続的に発生するような機構に対する逆動力学計算[5]，さらにこれらの機構のフィードフォワード制御[6]などが，アルゴリズムを変更することなく実行可能となった．ロボットの動作のさらなる高速化が求められている現状において，これは非常に重要な意義を持つものである．さらに，
本解法では動的な効果を含めた節点力を求めれば全関節トルクが算出されるため，剛体リンク系でも柔軟リンク系でも，その計算アルゴリズム自体に差異が生じない．この特徴を利用して，並列的逆動力学計算法に対し，有限要素によって弾性たわみを考慮した軌道計算アルゴリズムを付加することで，柔軟リンク系に対しても適用可能な解法が開発された[7]．
また，軌道計算と逆動力学計算を同一モデルで行うことにより，リンク機構に任意のタスクを与えた場合に，その軌道の算出から必要な関節トルクの算出までの過程を包括的に一つのアルゴリズムとして扱うことが可能になった．
本報告では，文献[7]の軌道計算アルゴリズムと逆動力学計算アルゴリズムに対し，線形チモシェンコ
はり要素と Bernoulli-Euler の理論に基づく 3 次はり要素の 2 種類の有限要素を導入，比較・検討し，計算コストの削減を図った．さらに，本軌道計算アルゴ
リズムの有効性を示すこと，また，フレキシブルマニピュレータのフィードフォワード制御の実現に必要なリンク部材の減衰定数を得ることを目的として，実機による検証を行った．

2.

軌道計算アルゴリズム

本節では，フレキシブルマニピュレータの逆動力学計算に必要となる目標軌道を作成するための，軌道計算アルゴリズムについて述べる．本アルゴリズ
ムは，骨組構造解析に広く用いられる有限要素法を，能動的に動作する機構に適用したものである．フレ
キシブルマニピュレータの挙動を，増分型の動的非線形問題として定式化し，その増分理論には非線形問題で一般的に使用される Updated Lagrangian Formulation (U.L.F)を用いた．また時間積分法としては，代表的な陰解法の一つである Newmark のβ法を
δ = 0.5 ， β = 0.25 として使用した．機構の移動によって生じる体積力変化を考慮すると，仮想仕事の原理より，時刻 t + ∆t における増分型運動方程式は以下のように定式化される．
[M]{u&&m}t+∆t +[M]{u&&d}t+∆t +[C]{u&d}t+∆t +[K]{∆ud}={F}t+∆t −{R}t{u m }t + ∆t = {um }t + {∆u m }{u d }t + ∆t = {u d }t + {∆u d }

(1)
(2)
(3)

Fig.1 1-link flexible manipulator used inthe experiments

ここで [M ]は全体質量マトリクス, [C ] は全体減衰マト
リクス，[K ] は全体剛性マトリクス，{F } は外力ベクト{um }は系の移動量のみのベク
ル，{R} は内力ベクトル，
トル，{ud }は系の変形量のみのベクトルである．また，減衰には比例減衰を用いることとし，[C ] を以下のように定義した．
[C ] = α [ M ] + β [ K ](4)

ここで用いる減衰定数αとβは，後に実験から求め決定する．式(1)に Newmark のβ法を適用すると次式が得られる．

ルクを算出する式を，全体座標系のマトリクス形式で表したものが次式である[4]．

11[M ] +[C ]){∆u d } = {F }t + ∆t − {R}tβ∆t 22 β∆t11111+[M]( {u&d}t +( −1){u&&d}t − 2 {∆um}+ {u&m}t +( −1){u&&m}t )2β2ββ∆tβ∆tβ∆t([ K ] ++ [C ]((11 − 4β− 1){u& d }t + (){u&&d }t ∆t )2β4βdmn(6)より，機構全体の移動量と変形量を含んだ変位量増分 {∆u} が得られる．このように，機構の変位量を移動量と変形量に分離して定式化することにより，系の動作に伴う遠心力などの非線形力項が考慮可能となる．

3.

{τ } = [L ][T ]{P }(5)

ここで，入力値となる {∆u m } ，Newmark のβ法から求まる速度・加速度成分 {u&}t , {u&d }t , {u&&}t , {u&&d }t を式(5)に代入することにより，変形量増分{∆u d } が得られる．最終的には次式{∆u} = {∆u } + {∆u }Fig.2 Connected joint

逆動力学計算アルゴリズム

本節では，リンク機構が目標軌道を実現するのに必要な関節トルクを算出するアルゴリズムについて述べる．本解法では，(1)式に各節点の加速度情報を入力して節点力を算出し，それを力学的な関係に基づいてトルクに換算する．剛性および減衰に関する項は，前節で算出した軌道上で変形量（すなわち節点力）として既に考慮されているため，逆動力学計算の段階では無視できる．得られた各節点力からトnnn(7)ここで，{τ n }は求めるべき関節トルクベクトル，{P n }は節点力に関するベクトルである．また，[T n ] は全体座標系からリンクの要素座標系に変換する座標変換マトリクス，[Ln ] は系の構造に関する部材長マトリクスである．このように，各変数をマトリクス形式に分離することにより，機構の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となる．

4.

数値的評価

本節では，前節までに述べた軌道計算アルゴリズ
ムと逆動力学計算アルゴリズムの計算精度と計算時間を評価するため，図 1 に示すような 1 リンクフレキシブルマニピュレータに対し解析を行った．図２にはリンク部材とモータとの結合部を示す．リンク
部材の拘束点がモータの軸から大きく離れるため，軸と拘束点との間に完全剛体に近い物性値をもつ要素を一つ設けることとした．リンク部材の物性値を表 1 に示す．リンク部材のモデル化は，計算コストの観点から，線形チモシェンコはり要素（LTBE）で２要素分割，また曲げ変形に対する精度の高い３次はり要素（CBE）で１要素分割の２通りで行った．
タスクは，リンク先端に 30[g]の質点を付加した上で，

Table 1 Parameter of Link member (Acrylic plastic)
0.4

Y(m)

Value
0.4
0.2×10-4
0.0952×10-1
3.2
6.667×10-12

0.2

X(m)

Parameter
Length: L [m]Sectional area: A [m2]Mass: m [kg]Young’s modulus: E [GPa]Moment of inertia: Iy [m4]

00.4

0.4

0.2

0.2

00-0.2-0.2

Converged solution
CBE 1 element
LTBE 2 elements

-0.2

Y(m)

01234time(sec)Fig.4 Comparison of trajectories for1-link manipulator
0.05

-0.4

-0.4X(m)-0.1 0 0.1 0.2 0.3 0.4 0.5-0.1 0 0.1 0.2 0.3 0.4 0.5(a) Rigid body model(b) Flexible model

Fig.3 Target trajectory obtained by FEM
図３のように水平面内を 2.0[s]でπ[rad]回転させ，その後 2.0[s]静止させるものを与えた．時間増分は，この部材の曲げ振動固有周期が 300[ms]であることを考慮し，十分に細かい 10[ms]とした．また，ここでは重力と減衰は考慮しないものとする．図３(a)には，初期値として入力した剛体リンク系の軌道を示す．
また，図３(b)には軌道計算アルゴリズムによって出力された軌道を示すが，リンクの部材剛性が考慮された結果となっていることがわかる．図４にはリン
ク先端の X 座標値を，図５には得られた軌道から算出したトルク曲線を示す．両図ともに，線形チモシ
ェンコはり要素(LTBE)２要素分割での計算結果は，収束解より周期が若干ずれていることがわかる．一方，3 次はり要素 1 要素分割での計算結果を見ると，両図において収束解に十分達していると言える．また，トルク曲線には高次の振動が表れているが，これは節点数が少ない故の数値的誤差が蓄積されたからと考えられる．さらに，3 次はり要素 1 要素分割での計算時間は 1.06[ms/step]であり，線形チモシェンコ
はり要素 2 要素分割の計算時間 1.40[ms/step]と比較すると，24%削減されることが確認された．以上より軌道，トルクともに，精度維持のためには 3 次はり要素 1 要素分割で十分であると判断し，計算コスト削減のためにこれを用いることとした．なお，本研究での計算は全て Pentium PC(CPU: 1.5GHz, memory: 512MB)で実施した．

5.

実機による検証
軌道計算アルゴリズムの有効性を検証し，減衰定Torpue(N*m)X(m)

0-0.05
Converged solution
CBE 1 element
LTBE 2 elements

-0.1

0

12time(sec)34

Fig.5 Comparison of torque curves for
1-link manipulator
数を得るために実機による検証を行った．図１に示した 1 リンクフレキシブルマニピュレータを用いて前節で行った数値解析と同様の条件で実験を行った．
実機が描く軌道は実機上方に設置したカメラにより撮影し，得られた画像から時々刻々の位置を計測した．また，観測誤差による誤差を小さくするため，最終姿勢における自由振動時の先端節点の座標値を，解析値との比較対象とした．図６に先端節点 X 座標の減衰を考慮しない解析結果，実験から求めた減衰定数を導入した解析結果，および実験結果をプロットした．減衰定数を導入した解析値は，実験値と良好に一致していることがわかる．また図７には，減衰を考慮しない場合と考慮した場合，さらに剛体リンク系の場合のトルク曲線を示す．フレキシブルマ
ニピュレータのトルク曲線は，部材の変形が考慮されているために，その部材が有する固有周期で剛体リンク系のトルク曲線を中心に振動していることがわかる．減衰を考慮したトルク曲線についても同様の傾向が見られ，本解法による計算結果の妥当性が示されたものと考えられる．

参考文献
0.4

[1] E. Boya, A Finite-Element Approach to Control theend-Point Motion of a Single-Link Flexible Robot, J. ofRobotic Systems,4(1), 63-75, 1987.

X(m)

0.2

[2] A. A. Shabana, Dynamics of Flexible Bodies Using
Generalized Newton-Euler Equations, Trans. of ASME, J.
of Dynamic Systems, Measurement, and Control, 112,496-503, 1990.

0Analysis without damping
Analysis with damping
α=0.14 β=1.0∗10−4Experiment-0.2
012time(sec)

34Fig.6 Target trajectory[4] 磯部大吾郎, “有限要素法を用いたリンク機構の逆動力学計算”, 日本ロボット学会誌, 20(6), 647-653,
2002.

Torque(N*m)

0.05

0

[5] D. Isobe, D. Imaizumi and A. Yagi, A Parallel Solution Scheme for Inverse Dynamics of Link Mechanisms,
JSME International Journal Series C, 46(2), 766-771,
2003．

-0.05
Rigid body modelAnalysis without damping
Analysis with damping
α=0.14 β=1.0∗10−4-0.1012time(sec)34

Fig.7 Torque curves

6.

[3] H. Asada, Z. –D.Ma and H. Tokumaru, Inverse Dynamics of Flexible Robot Arms: Modeling and Computation for Trajectory Control, Trans. of ASME, J. of Dynamics Systems, Measurement, and Control, 112, 177-185,
1990.

結言

本研究では，様々なリンク系に対する統一的逆動力学計算法の開発の一環として，フレキシブルマニピュレータの逆動力学計算法を構築し，その計算コストのさらなる削減を図った．また，その軌道計算アルゴリズムの有効性を実験により確認することができた．
今後は，減衰を考慮したトルク曲線を用いて，フレキシブルマニピュレータのフィードフォワード制御を実現することが課題となる．

[6] D. Isobe, Y. Chikugo, D. Imaizumi, S. Sato and A.
Yagi, Feed-forward control of link mechanisms undervarious boundary conditions by using a parallel solution
scheme, Proc. 2003 IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS 2003), 2003,2773-2778, Las Vegas U.S.A.
[7] 磯部大吾郎, 今泉大作, “リンク系の部材剛性に依存しない統一的逆動力学計算法”，日本機械学会論文集（C編）,70(691), 728-735, 2004.