汎用を目的とした逆動力学計算法の開発
筑波大学○磯部大吾郎

佐藤 俊介

八木 淳

Development of Inverse Dynamics Solution Scheme
for Purpose of General Use
Univ. of Tsukuba ○Daigoro ISOBE, Shunsuke SATO and Atsushi YAGI
Abstract: Generally, robotic tasks include motions that generate open and closed loops alternately, and the dynamic
equations of the system (or the numerical algorithm) require instant switching during the motion in feed-forward control.
The switching may lead to instability of the control, and the issue will become essential particularly for massive, quick
motion robots. A new solution scheme of inverse dynamics is developed for the purpose of general use, in order to ensure thesimplicity in dealing with such transforming systems or those with complex configurations, or the calculation of Jacobianmatrices in force controls. In this paper, we describe some application cases on feed-forward control of multi-branch linksystems, and on force control of two manipulators holding an object in a motion much affected by their dynamics.
Key Words: Feed-forward Control, Force Control, Inverse Dynamics, Link Systems, Parallel Solution Scheme

1. はじめにトルクを求めるという，全く新しい発想に基づいた

近年，ロボットに対し，その動作高速化の需要が逆動力学計算法を開発した[1]．3 次元に拡張されますます高まっている．一方，高度な要求が機構のた本解法はフィードフォワード制御系にも適用さ複雑化につながり，例えばその動力学方程式を導出れ，その有効性が立証された[2]．その特徴は，系する過程が，初学の研究者（一般ユーザ）にとって全体を有限要素により離散モデル化後，直交座標系はハードルの高い存在となっているのが現状であで表記された全体座標系での連立方程式を解き，モる（図１参照）．同様に，このような複雑な機構にデル内の節点に作用する節点力を並列的に求め，こ対し力制御をしたい局面では，ヤコビアンの導出にれを各パラメータに分離されたマトリックス形式困窮する一般ユーザが多いのではないだろうか．確の換算式に代入し，トルクを出力するという点にあかに，上記のような個々の局面に対処するための要る．モデルの形状と目標軌道の入力のみで様々なリ素技術は存在する．各々の場合でその技術を使い分ンク系の動作に対応し，また動力学方程式に相当すけ，問題を解決することは可能である．しかし今後，る部分の書き換えを一切必要としないため，ユーザ一般ユーザの裾野が広がり，ロボット（またはそのに対する簡便性を維持することが可能である．他方，動作）に対する要求がますます多様化した場合，要本解法は増分的に解を算出する近似解法であるた素技術がこのように多極化した状態で果たして真め，実際に制御系に組み込んでも精度と計算時間のの意味でロボットは発達するであろうか．

観点から問題は無いものの，厳密解を与える動力学

筆者らは，ロボット工学や制御工学の知識をあまり持たない技術者に対し，簡易でかつ汎用的な方法方程式を用いた方法に比べ３倍程度の計算時間を要することが確認されている[2]．

を提供することが，今後のロボットの発達を促進させるものと考える．システムや要素技術のインテグレーションによる汎用化はもちろん可能であるが，本研究では，理論の統一による汎用化を目指す．その観点で，力の次元の運動方程式を並列的に解いてFig.1 Cases difficult to handle for general users?

理論の詳細については他文献[1][2]に任せ，本稿

ここで，[Ln]は部材長マトリックス，[Tn]は座標変換

では動力学方程式を用いた従来の手法との相違をマトリックス，{Pn}は節点力に関するベクトルであ中心に解説し，複雑系の一種である枝分かれリンクる．右上添え字は総リンク数を示す．上式は，パラ系の数値例およびフィードフォワード制御結果にメータ毎に独立のマトリックスまたはベクトルについて述べる．さらに，2 本のマニピュレータを使分離された形となっており，モデルの形状を表す入い，一定の力で物体を把持したままそれを振り回す

力データを操作するだけで構造変化に柔軟に対応という，動力学補償と発生トルクの同時算出を必要可能である．計算の手順としては，時々刻々の節点とした力制御実験結果について報告する．

力増分が{Pn}に入力され，全体座標系から要素座標系へ変換の後，部材長成分が掛けられてトルクが求

2. 並列的逆動力学計算法

められる．

2.1 動力学方程式を用いた解法との比較

次に，節点力の算出過程について簡単に説明する．

一般にリンク系の動力学方程式はニュートン･オイラー法やラグランジュ法を用いて導出され，次式

力の次元の運動方程式から節点力増分は以下のように導出される．のような形となる[3]．
° V (í; í_) + G(í)ú= M (í)í+

fÅf g = fRgt Ä fF gt +(1)[M ](111fug_ tÄ(Ä 1)f°ugt ) (3)fÅug Ä
åÅt2
åÅt2å

ここで，右辺第一項は慣性力項，第二項は遠心力およびコリオリ力項，第三項は重力項である．また，ここで，[M]：全体質量マトリックス，{F}：外力ベ_ ，í°はそれぞれ，リンク間の相対回転角，相対í，íクトル，{R}：内力ベクトル，fÅug：変位増分ベク

回転角速度，相対回転角加速度である．すなわち，_ ：速度ベクトル，f°
ug：加速度ベクトル，トル，fug

上式は相対回転座標系に基づいて定式化されるたΔt：時間増分，β：Newmark のβ法[7]の積分パめ各パラメータが相互に依存し合い，リンク系の構ラメータ（本研究では 1/4 を採用）である．変位増造が変化した場合には方程式の大部分の書き換え分ベクトルとしては，目標軌道の各ステップ間の移が必要となる．さらに，閉ループ系や枝分かれ系な動量が入力される．速度・加速度ベクトルは外部か

どの複雑なリンク系の場合には方程式の導出そのらの直接入力も可能だが，ここでは Newmark のβものが困難となり，特殊な工夫が必要となる[4][5]．

法により逐次的に算出したものを使用する．部材変中村らは，ポインタを使ってリンク系の構造を表現形が生じないために，内力ベクトルの各成分は実際し，効率的な順動力学･逆動力学計算アルゴリズムにはゼロとなる．また，外力ベクトルには自重や付を開発した[6]．しかし，上式のようなトルクベー加的な作用項が含まれる．上式にしたがって算出さスの動力学方程式に基づいているため，閉ループ系れた節点力増分が逐次的に足し合わされ，(2)式の節の場合にはループの仮想的切断の操作とともに，ヤ点力に関するベクトル{Pn}に組み込まれる．詳細にコビアンの導出を常に必要とする．

ついては文献[1][2]を参照されたい．

一方，並列的逆動力学計算法では，以下の式によってトルクを算出する[1][2]．

図2 に両解法で3 リンク機構に対して行った逆動力学計算結果を示す．厳密解を与える動力学方程式による解法は計算ステップ数に依存しないのに対

nnnnfú g = [L ][T ]fP g(2)し，近似解法である並列的解法はこれに依存することが分かる．そこで，実際の制御においては，十分Torque[N･ m]

5Z[m]1100 steps
50 steps
20 steps
10 steps

3Joint 1-50.50a0200.20.4

0.60.81(a) OpenOpen-loop

Time[s]1(b) Conventional scheme

X[m]
00.5

1(a) Target motion

Torque[N･ m]

5-0.5

100 steps
50 steps
20 steps
10 steps

a1am
a20・・・

Joint 1-500.2

0.4

0.6

0.8

(b) ClosedClosed-loop

1Time[s](c) Proposed scheme

Fig.. 2 Accuracy of torque curves against number
Figof steps

aイムを設定する必要がある[2]．
2.2 部材長マトリックスによる機構構造の表現
部材長マトリックスは，長さの次元の成分によって構成され，力をトルクの次元に変換する役割を担

bii-1

なステップ数が確保されるようにサンプリングタ(c) MultiMulti-branch
Fig.3 Several types of system configur
configurationsationsう．また，成分の並び方によってリンク系の結合状ンク系（図 3(b)参照）に対しては，部材長マトリッ態をも表現し，トルクの重ね合わせを行う．図 3(a)クスは次式のようになる．に示すように a 本のリンクで構成される開リンク2系の場合には，次式のような上三角行列となる．
266hi 66a=6L664L1L2L20ÅÅÅ LaÄ 1
ÅÅÅ LaÄ 1..
..
.
.
LaÄ 1

37La 77.. 7. 777La 5Lah(4)La

Lni6666=66643

L a1L a20..

0.
LamÄ 1L am77777 (5)775

ここで La1,La2,･･･,Lam などのマトリックスは，各々が(4)式で与えられるような上三角行列となる．
すな

ここで，個々の成分 Li (i=1,･･･,a)の右下添え字は i

わち，トルク値が重合されるべき直列構造は(4)式の番目のリンクに関するマトリックスであることをような上三角行列で表され，トルク値が重合されな示し，このマトリックスには重心までの距離やリンい並列構造は，(5)式のように各々の上三角行列が対ク長などの成分が入る[1][2]．
次に，a1 本，a2 本，･･･，角に並ぶ行列で表されるのである．以上のことを踏am 本(a1+ a2+･･･+am=n)のリンクが連結された閉リまえると様々な構造に対する表現が可能となり，例

えば，図 3(c)に示す枝分かれリンク系に対する部材長マトリックスは以下のようになる．
2666666hi 66Ln = 666666664

La1

ÅÅÅ Lai..
..
.
.
Lai

ÅÅÅ Laa..
.
ÅÅÅ Laa..
..
.
.

Lb1..
.
Lb1

Laa
Lb103
ÅÅÅ Lbb.. 77. 77
ÅÅÅ Lbb 777707777b 7ÅÅÅ Lb 7. 7..
. .. 75Lbb(6)(b) Proposed scheme(a) Dynamic equations

Fig.4 Estimation of torque curvesな例をあえて選んで比較したが，これ以上複雑なリンク系になった場合，一般ユーザにとってその導出過程が難解となることは容易に想像できる．一方，上式は，a リンク系の i 番目の関節から b リンク系

並列的解法では，モデルの形状と目標軌道を入力す(a+b=n)が枝分かれしている状態を表す．基本的にるのみでトルクの計算が実行される．前述の部材長は 2 本の独立した開リンク系の形（2 つの上三角行マトリックスはアルゴリズム内で自動的に構成さ列）を成すが，枝分かれをする部分（i 行目）よりれるため，煩雑さを伴わない．

根元側に b リンク系の 1 行目が付加され，トルク値次に，
図 5 に示すような枝分かれ閉リンク系に対が重合される．このように，部材長マトリックスはリンク系の結合状態を陽に表現でき，自動的に成分を構成するのに適している．

Mass

2.3 力制御への適用

3

動力学補償と発生力とを同時に必要とする制御

24Joint 1

Pin jointの場合，従来の方法では，動力学方程式から求めたトルク値と発生力ベクトルにヤコビアンを掛けて求めたトルク値とを足し合わせる．この際，リンク系の動作に合わせてヤコビアンを時々刻々計算する必要がある．

Fig.5 MultiMulti-branchbranch link system

一方，並列的解法は力の次元の運動方程式を解く
Torque[N・m]ため，発生力をそのままトルク計算に入力することが可能となり，力制御の際に改めてヤコビアンを導出するなどの操作を必要としない．具体的には，(2)式の{Pn}ベクトルの該当する箇所に発生力成分を入力すれば良い．
3. 枝分かれリンク系のフィードフォワード制御
図 4 に簡単な枝分かれリンク系に対する逆動力学計算結果を示す．図 4(a)は動力学方程式による解，
図 4(b)は並列的解法による解である．両者は良好に一致することが分かる．動力学方程式の導出が容易

0.04

0.02

0-0.02

Joint1
Joint2-0.04
02

Joint3
Joint446

Time[s]multi-branch link
Fig.6 Torque curves for multisystem

Angle[rad]Feedbackのみの場合とFeedback+Feedforwardの1.58場合とを比較すると，後者の方が目標（つまり静止軌道）に対する追従性が向上していることが確認できる．

1.57

4. リンク系のフィードフォワード
リンク系のフィードフォワード力制御
フィードフォワード力制御
図 8 に示すようなモデルに対し，一定の力で押しながら壁をなぞらせる際のトルク曲線を図 9 に示

：Target：FB：FB+FF

1.56
0246Time[s]す．動力学方程式によって求めたトルクを破線，ヤ8

コビアンを発生力に掛けて求めたトルクを点線に

Fig.7 Control result (Joint 1)

示すが，両者を足し合わせたものが従来の手法によし，並列的解法を導入した制御系で制御実験を行った．4 つのモータにはギアを介さず，動力学の影響を相対的に大きくした．枝リンクの先端に質点を配して左右に振り，その間，閉リンク部を保持させるように制御した．図 6 に制御中に算出されたトルク曲線，図 7 に Joint 1 における角度曲線を示す．

る解となる．図は，この解と並列的解法による解が一致することを示している．
次に，図 10 に示すような位置フィードバック(FBp)，力フィードバック(FBf)とフィードフォワード(FF)を組み合わせた力制御系を設計し，これに並列的解法を導入した．本制御系では，PC 上で直交座標系に基づく目標軌道が入力された後，並列的解法により逆動力学計算が行われ，逐次的に全関節のトルク値τI.D.が算出される．その際には，サンプリ

RollerZFEZ
θ２ｚFEれ，計算終了後に各関節のトルク値が出力される．
例えばサンプリング周期を 10 [ms]とした場合，こJoint2

Link2

ング周期ごとにその時間分の計算が増分的に行わ

θ１の間のトルク値は約 0.7 [ms] (Celeron 1.1GHz)でLink1算出されることが確認されている．次に，モータにJoint1XXθ １ θ ２： Rotational angle of Link1,Link2

発生する摩擦力の影響やフィードバック値などを加え，最終的な制御トルクτとする．位置フィード

F E：Generating Force

バック(FBp)値と力フィードバック（FBｆ）値は，Fig.8 Simulation model

それぞれ次式により求めた．

Torque[Nm]0.15

0.1

0.05

X, Z, Qd : Coordinates in global Cartesian coordinates

0

Proposed scheme
Inverse Dynamics[J]T{F}
Conventional scheme

00.2

0.4

0.6

0.8

1

T[s]

Fig.9 Torque curves for simulati
simulation
on modelFE, FS: Target force, actual force

qd , q_d : Target angle and angular velocity

q , q_KF: Actual angle and angular velocity: Feedback gain for force

Fig.10 Outline of force control system

Torque[Nm]Object
0.05

Joint1
Joint2
Joint3
Joint402

4-0.05

3

Joint 1-0.1

Fig.11 Two manipulators holding an object01234T[s]Fig.13 Torque curves for two manipulators
in force control
Torque[Nm]0Inverse dynamics
Torque for generated force-0.02

Joint 1-0.04

-0.06

-0.08

Fig.12 Target motion for two manipulators
in force control

01

2

3

4T[s]

Fig.14 Comparison of dynamics in torque curves

標軌道（動作時間：4 秒），図 13 に実験中に算出
úf eedback = Ku (qd Ä q) + Ki Ü(qd Ä q) + Kv (q_d Ä q)_(7)

されたトルク曲線，図 14 に Joint 1 での逆動力学

Ff eedback = KF (FE Ä FS )

この場合，逆動力学は決して無視できる大きさでは(8)と力を発生させるためのトルク値との比較を示す．
なく，その正確な計算が重要であることが分かる．

ここで，qd ，q_d はモータの目標角度および目標角FBｆ値を優先させているため位置制御に多少の誤速度，q ，q_ はエンコーダより取得した実際の角度差があるが，図 15 の制御結果を見ると，全体的におよび角速度，FE は目標発生力，FS は力覚センサ

良好な目標追従が得られることが確認された．

より取得した力の値，Ku ，Ki ，Kv，KF はそれぞれ角度，角度の積分値，角速度，力に対するフィードバックゲインである．

5. 最後に
本研究では，汎用を目的として開発された並列的並列的解法を導入した力制御系を用い，図 11 に逆動力学計算法を，枝分かれリンク系のフィードフ示す 2 本のマニピュレータで物体（木片）を把持し，ォワード制御と複数マニピュレータのフィードフ把持力を一定（0.5 [N]）に保ったまま物体を振り回ォワード力制御に適用し，その有効性を実証した．

す制御実験を行った．本実験でも 4 つのモータには本解法の汎用性は，力の次元の運動方程式を並列的ギアを介さず，動力学の影響を相対的に大きくした．に求解することで実現される．また，動力学方程式

図 12 に物体を把持した２本のマニピュレータの目やヤコビアンなどの導出という，煩雑な操作を一切

10.70.5
TargetControl result
0Joint3

Rotational angle[rad]Joint2
Rotational angle[rad]Force[N]0.8

10.60.50TargetControl result

0.40.3

Joint10.2

Joint4-1-0.50123(a) Angle (Joint 1,2)

4 T[s]

TargetFF+FBp+FBf

0.10123(b) Angle (Joint 3,4)

4 T[s]

00123(c) Generated force

4

T[s]

4--second motion
Fig.15 Control results in a 4

必要としない．そのため，一般ユーザにとっても受[3]け入れやすい解法であるものと思われる．
[4]
参考文献
[1]
[2]

磯部大吾郎: “ 有限要素法を用いたリンク機構の逆動力学計算”，日本ロボット学会誌, vol.20,
no.6, pp.647-653, 2002.
Isobe, D., Chikugo, Y., Imaizumi, D., Sato, S. andYagi, A.: “Feed-Forward Control of Link
Mechanisms under Various Boundary Conditions
by Using a Parallel Solution Scheme”, Proc. 2003
IEEE/RSJ Int. Conf. Intelligent Robots andSystems IROS’03, (2003), pp.2773-2778, Las
Vegas U.S.A.

[5][6][7]

Craig, J. J., 三浦宏文・下山 勲 訳：ロボティク
ス -機構・力学・制御-, pp.155-188, 共立出版,
1991.
中村 仁彦：“ パラレルメカニズムの動力学” ,日本ロボット学会誌，vol.10, no.6, pp.709-714.
杉本 浩一： “ 閉ループ機構の運動方程式の導出”，日本ロボット学会誌，vol.15, no.3,pp.460-467, 1997.
中村 仁彦，山根 克，永嶋 史朗: “ 構造変化を伴うリンク系の動力学計算法とヒューマンフィギュアの運動計算”，日本ロボット学会誌,vol.16, no.8, pp.1152-1159, 1998.
Newmark, N. M.: “ A Method of Computationfor Structural Dynamics ” , A.S.C.E. J.
Engineering Mechanics, vol.85, pp.67-94,
1959.