䢴䣃䢳䢯䣊䢴䢵

並列的解法による劣駆動リンク系の逆動力学計算
Inverse Dynamics Calculation of Underactuated Link Systems Using Parallel Solution Scheme○北村 悠人（筑波大院） 山中 浩司（筑波大院） 正磯部 大吾郎（筑波大）

Yuto Kitamura, Kouji Yamanaka (Graduate School, Univ. of Tsukuba) and Daigoro Isobe (Univ. of Tsukuba)
In this paper, the parallel solution scheme is applied to the calculation of inverse dynamics for underactuatedlink systems. This paper describes the process of a solution scheme of kinematics that is developed on the basisof the finite element method in order to calculate target trajectories that compensate for the inertial forces
acting in the systems. The obtained trajectories are fed into the inverse dynamics calculation using the parallelsolution scheme. Some numerical experiments are carried out on underactuated link systems and the validity ofthe parallel solution scheme is verified.
Key Words: Inverse dynamics, Underactuated link systems, Kinematics, Parallel solution scheme
1. はじめに本研究で扱う劣駆動リンク系とは、システムの自由度に対してアクチュエータの数が少ないシステムで、アクチュエータにより直接制御できない関節が存在するリンク系のことである。劣駆動リンク系の利点として、系の軽量化・省電力化・アクチュエータが少なくて済むことによるシステムの簡素化・耐故障性の向上およびコストダウンなどが挙げられる。
そのため、宇宙空間など様々な分野での利用が期待されている。しかしながら、直接制御できない関節が存在するため、非ホロノミックなシステムとなり、制御は難しくなる[1][2]。
一方、連続体力学に基づく数値解析手法として広く用いられている有限要素法（FEM）は、一般的に絶対的な直交座標系に基づいて定式化され、系全体を微小要素に離散化し、それぞれの情報を全体方程式に集約して解を求めるという手法である。この特徴を利用し、並列的に求められた節点力を力学的な関係に基づいて関節トルクに変換する並列的逆動力学計算法（以降、並列的解法と記す）が開発された[3]。本
解法の節点力‐トルク換算式では、節点力に関する成分、座標変換に関する成分、部材の長さに関する成分が個々のマトリックスに分割されて表現されるため、機構の変化に対しても入力データの変更により柔軟かつ簡便に対応することができる。また、柔軟リンク系や枝分かれリンク系など、複雑なダイナミクスを有する系やロボット機構に対しても適用可能であることが確認されている[4-6]。
本研究では、劣駆動リンク系の逆動力学問題に焦点を当て、まず劣駆動リンク系の軌道を算出するためのアルゴリズムを FEM に基づき開発した。また、これにより得られた軌道を並列的解法による逆動力学計算に入力し、算出されたトルク曲線から並列的解法の妥当性を確認した。
２．軌道更新アルゴリズム
並列的解法は、動的な効果を含めた節点力を求めれば全関節のトルクが算出されるため、その計算過程は部材剛性に依らず同一のものとなる。したがって、部材剛性や減衰を考慮した正確な軌道を作成し、それを並列的解法の入力とすれば良い。以下に FEM を用いた軌道計算アルゴリズムについて記す。系の移動によって生じる体積力を考慮すると、仮想仕事の原理より、時刻 t+⊿t における増分型運動方程式は次式で定式化される。

[M]{u&&m}t+∆t +[M]{u&&d}t+∆t +[C]{u&d}t+∆t +[K]{∆ud} ={F}t+∆t −{R}t(1)ここで[M]は全体質量マトリクス、[C]は全体減衰マトリクス、[K]は全体剛性マトリクス、{F}は外力ベクトル、｛R｝は内力ベクトル、{um}は系の移動量ベクトル、{ud}は系の変形量ベクトルである。(1)式に時刻毎の{um}を入力することで逐次的に{ud}が算出され、この 2 つの変位量の和として剛性や減衰を考慮した軌道が得られる。時間積分法としては、Newmarkのβ法(δ=1/2,β=1/4)を使用した。さらに本研究では、劣駆動関節を表現するために、(1)式の全体剛性マトリクス[K]の軸方向以外の成分を 0 にすることとした。一方、劣駆動関節が系の中に存在すると、初期の入力軌道（剛体リンク系のもの）と出力軌道（劣駆動リンク系のもの）との間に大きな差異が生じてしまうため、正確な軌道が算出できない。その解決法として、入力軌道である剛体リンク系の軌道をステップ間で逐次更新することとした。本稿では、水平面内における２リンク系モデルを例として、この軌道更新アルゴリズムについて説明する（Fig. 1 参照）。まず、任意の時刻 t の入力軌道が分かっているとし、この入力軌道を[K]マトリクスの軸方向以外の成分を 0 とした(1)式に入力し、２リンク目に生じる変形量{ud}t を算出する。次に、リンク先端の変形量から劣駆動関節の変形角θt を算出し、ｋの変形角θt を保持したま
ま、１リンク目の関節を剛体リンク系の入力軌道通りにφt+1Ｙｔ-1ｔ

θ t −1

θ t −1θtθ t +1ｔ+1θt
ϕt −1ϕt
ϕ t +1

Initial trajectory
Input trajectory at each time step
Revised trajectory

Ｘ
Fig. 1 Concept of successive trajectory revision algorithm forunderactuated link systems䣝䣐䣱䢰䢢䢲䢺䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢺䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣐䣣䣩䣣䣰䣱䢮䢢䣌䣣䣲䣣䣰䢮䢢䣌䣷䣰䣧䢢䢷䢯䢹䢮䢢䢴䢲䢲䢺䢴䣃䢳䢯䣊䢴䢵䢪䢳䢫

へ回転させることにより、時刻 t+1 における入力軌道を決定する。０step 目の各節点の初期速度および初期加速度については、初期の剛体リンク系のものを用いることとする。
３．並列的逆動力学計算法
並列的解法では、入力データとして与えられる軌道より陽に算出される節点力を、力学的な関係に基づいてトルクに換算する。節点力からトルクに換算し、マトリクス形式で表したものが次式である。

{τ }= [L ][T ]{P }nnnn(2)

ここで、{τn}は求めるべき関節トルクベクトル、{Pn}は節点力に関するベクトル、[Tn]は全体座標系からリンクの要素座標系に変換する座標変換マトリクス、[Ln]は系の形態を表す部材長マトリクスである[3-6]。このように各変数をマトリクス形式に分離することにより、系の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となる。
４．劣駆動リンク系の数値例
数値例として、2 リンク劣駆動リンク系の軌道計算および逆動力学計算を行う。2 リンクともに長さ 0.25[m]、質量 97.5×10-3[kg]、ヤング率 2.0×1011[N/m2]、断面二次モーメント1.042×10-10[m4]とし、2 リンク目の先端に 0.1[kg]の付加質点を設けた。初期の入力軌道（剛体リンク系のもの）は、2 リ
Y[m]
0.4

J20.05J1Torque[N*m]Joint1
Joint2５．結論
本研究では、並列的解法を用いて劣駆動リンク系の逆動力学計算を行った。その結果、得られるトルク曲線から、並列的解法が劣駆動リンク系に対しても適用可能であることを確認した。今後は、多リンク劣駆動リンク系への適用および実機による制御実験などを予定している。

0.200

謝辞
本研究を遂行するに当たり、（独）日本学術振興会科学研究費補助金基盤研究 C（課題番号: 19560250）および（財）メカトロニクス技術高度化財団の助成を受けた。ここに謝意を表する。

-0.2
X[m]
0-0.0500.2

Fig. 2(a) Calculated trajectory
for a two-joint underactuatedlink system considering thestiffness of Link 10.5

1Time[s]1.52Fig. 2(b) Torque curves for a
two-joint underactuated link system
considering the stiffness of Link 1

Y[m]
0.4
0.05

0 J1

Torque[N*m]Joint1
Joint2

J2
0.2

0-0.2
X[m]
00.2

-0.05
00.5

1Time[s]1.5

ンク目の関節 J2 は固定したまま、水平面内を根元の関節 J1のみが 2 秒間でπ[rad]回転する軌道とする。減衰は考慮せず、時間増分 10[ｍs]の 200step で計算した。軌道更新アルゴリズムにより得られた軌道を Fig. 2(a)に示し、得られた軌道を並列的解法に入力し、算出されたトルク曲線を Fig. 2(b)に示す。
図から、2 リンク目のトルクがほぼ 0 の値を維持していることが分かる。すなわち、軌道通りに動くためにトルクを必要としない、劣駆動関節を正確に表現できていることを示している。さらに、1 リンク目のトルク曲線の振動はこのリンク系の固有振動であり、その周期はリンク系の姿勢により0.025[s]～0.04[s]の間で推移する。これは、片持ち梁の自由振動の場合の理論解 0.031[s]とほぼ一致する。つまり、1 リン
ク目のトルク曲線に現れる振動は、部材剛性を考慮したために発生した弾性振動に起因する。これを実際に制御に適用する場合には、この振動は制御の安定性を損う原因となる。そこで、部材変形量に対する速度、加速度ベクトルを人為的に0 とすることで、1 リンク目を剛体に理想化することとした。
その結果、Fig. 3(a)の軌道および Fig. 3(b)のトルク曲線が得られる。Fig. 2(a)と Fig. 3(a)を比較すると、算出された軌道はほぼ一致する。また、Fig. 2(b)と Fig. 3(b)を比較すると、Fig.
2(b)で見られた１リンク目の振動は Fig. 3(b)の曲線上で振動していたことが分かる。Fig. 3(b)の初期トルクに誤差が見られるが、この原因としては、１リンク目の部材剛性を考慮しなかった点、各節点における初期速度・初期加速度を入力軌道の剛体リンク系のものとした点が挙げられる。しかしながら、この程度の誤差であれば、制御を行う際には重大な影響を及ぼさないものと思われる。

2

Fig. 3(a) Calculated trajectory Fig. 3(b) Torque curves for a two-joint
for a two-joint underactuated underactuated link system without
link system without considering considering the stiffness of Link 1
the stiffness of Link 1

参考文献[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] D. Isobe, A. Yagi and S. Sato: General-Purpose Expression of
Structural Connectivity in the Parallel Solution Scheme and Its
Application, JSME International Journal Series C, 49(3),pp.789-798, 2006.
[5] 磯部大吾郎，加藤 昭博：フレキシブル・リンク系のモデ
ルベースド・フィードフォワード制御，日本ロボット学
会誌，25(4), pp.625-631, 2007.
[6] D. Isobe and K. Yamanaka: Development of a Unified
Feed-Forward Control System for Robotic Mechanisms using
Finite Element Approach, CD-ROM Proceedings ofAPCOM'07-EPMESC XI, 2007, Kyoto, Japan.

䣝䣐䣱䢰䢢䢲䢺䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢺䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣐䣣䣩䣣䣰䣱䢮䢢䣌䣣䣲䣣䣰䢮䢢䣌䣷䣰䣧䢢䢷䢯䢹䢮䢢䢴䢲䢲䢺䢴䣃䢳䢯䣊䢴䢵䢪䢴䢫