1I24

Projected Gauss-Seidel 法の拡張による剛体関節機構の接触力計算○中岡慎一郎 服部静子 金広文男 梶田秀司 比留川博久 (産総研)

Iterative Contact Force Solver for Simulating Articulated Rigid Bodies*Shin’ichiro NAKAOKA, Shizuko HATTORI, Fumio KANEHIRO,
Shuuji KAJITA and Hirohisa HIRUKAWA (AIST)
Abstract— We developed a simulation system that can achieve realistic and efficient simulations of articulated rigidbodies. This paper focuses on our constraint-based contact force solver where the force solution is calculated by an
iterative method based on the Projected Gauss-Seidel method. In this solver, accurate frictional force can be calculated
from a simple constraint formulation. The method was verified by several simulation tests that include the comparison
between the actual and simulated motions of HRP-2, and the results show the validity of the method.
Key Words: dynamics simulation, articulated rigid body, contact force, LCP, Projected Gauss-Seidel Method

1. はじめに
近年，複雑な関節機構をもつロボットを対象として，より高度な動作の実現を目指す研究が盛んであり，そのような動作で発生する複雑な接触を安定・正確・高効率に処理できるシミュレータが求められている．
接触によって発生する力を計算する一般的な手法のひとつはペナルティ法である [1, 2]．これは物体同士の微小なめり込みを許し，めり込みの量に応じた反発力を発生させるというものであり，実装が容易で計算効率が高いという特徴をもつ．しかし，この手法ではロボットのリンクとして一般的な剛性の高い物体の接触は正確にシミュレートできない．また，シミュレーションの安定性がパラメータ設定や積分手法・積分間隔に強く依存し，この点で扱いづらいという問題がある．
剛体同士の接触を正確にシミュレーションするため，物理的な拘束条件に基づいて接触力を計算する手法が提案されている [3, 4, 5]．ただし多くの手法は慣性行列の逆行列を直接計算に用いるため，関節機構の一般化座標を用いると機構の自由度 n に対して O(n3 ) の計算量が必要となり，自由度の多いロボットに対する効率が悪い．この問題を解決する手法を Kokkevis[6] やChardonnet ら [7] が提案しているが，これらの提案では正確な摩擦力を実現するための定式化とその解法について不明確な点が多い．
本論文では Kokkevis の手法をベースとした上で，摩擦力も正確に計算可能な解法の提案を行う．またこの手法の閉リンクシミュレーションへの応用についても述べる．本論文の手法により，正確な剛体接触シミュレーションを，自由度の多いロボットに対しても効率的に行うことが可能となる．

2. 拘束条件の定式化
2·1

拘束点と拘束ベクトル

本手法では，剛体間の接触や接続が有限個の拘束点と各拘束点に設定される線形独立な複数の拘束ベクトルによって表現されるものとする．各拘束点にかかる   

!"$#%!&'( &")!

!&$'!+*, 

Fig.1 Constraint Vectors

拘束力は各拘束ベクトルの成分に分解して記述される．
ある状態における拘束ベクトルの総数を m ∈ ℜ とし，i (≤ m) 番目の拘束ベクトルを ci ∈ ℜ3 , (|ci | = 1)，このベクトルに対応する拘束点を pi ∈ ℜ3 とする．
Fig.1(a) は剛体同士が点接触する際の拘束点・拘束ベクトルの例を示している．c1 は接触法線方向に設定される拘束ベクトルで，抗力に対応する．また摩擦がある場合は，c2 , c3 のように，摩擦力に対応する拘束ベクトルが接平面において直交する２軸として設定される．線や面にわたる接触もいくつかの点接触の組として表されるものとする．シミュレータにおいて接触点は通常幾何干渉検出プログラムの出力から得る．
接触とは異なる拘束の例として，Fig.1(b) に示すような，剛体同士をある相対位置で接続するための拘束点も設定可能とする．この拘束により，開リンクの一部を接続して閉リンクを構築することができる．この場合，拘束ベクトルの本数は拘束自由度に応じて決定する．

2·2 拘束力と加速度の関係
拘束ベクトル ci 上で作用反作用の関係にある拘束力の一方を fi ci , ( fi ∈ ℜ) とする．ただし fi は物体同士を反発させる方向を正とする．また，拘束力を含めた物体にかかる力によって拘束点 pi に発生する相対加速度の ci 成分を ai ∈ ℜ とする．すると， fi を並べたベクトル f = ( f1 f2 · · · fm )T と ai を並べたベクトル

第25回日本ロボット学会学術講演会（2007年9月13日〜15日）

a = (a1 a2 . . . am )T に対して，以下の線形式を得ることができる．
a = Af + a0
(1)

次に，拘束ベクトル cx , cy がある接触に付随する摩擦拘束の２軸である場合を考える．まず摩擦力が無限に働くと仮定すれば，拘束条件としては

行列 A ∈ ℜm×m は各拘束力が各拘束点におよぼす加速度の割合を示しており，ベクトル a0 ∈ ℜm は拘束力以外の力による各拘束点の加速度を示している．
Kokkevis の手法 [6] によって，関節機構に対して式(1) の定数要素を効率的に計算することができる．まず，
f = 0 とした状態で順動力学を計算し，その結果得られる a の値を a0 とする．次に，行列 A の i 列要素 αiを以下の式によって得る．

(vx vy )T = 0

αi = ai − a0(2)

ここで ai は拘束力を以下のように設定した順動力学によって得られる a の値である．
1 j = ifj =(3)
0 j 6= i.

Kokkevis の手法では，以上の処理における順動力学計算に Featherstone の Articulated Body Algorithm (ABA)[8] を用いる．ABA は関節機構の自由度 n に対して O(n)の計算量を実現しているため，これを用いることで式(1) の定数要素を求めるのに必要な計算量は O(nm + m2 )となり，関節機構の自由度に対して線形の計算量に抑えることができる．
2·3

離散化による拘束力と速度の関係

運動を微小時間 ∆t で離散化し，式 (1) にオイラー積分を適用することで以下の式を得る．

v = ∆t(Af + a0 ) + v 0(4)

ここで v 0 , v はそれぞれ積分前・積分後の拘束点 pi における相対速度の ci 成分を並べたベクトルである．拘
束条件はこの式の v と f に関して記述される．
簡単のため，以下では式 (4) において ∆tA = K,∆ta0 + v 0 = b として書き換えた次の式を用いる．

v = Kf + b(5)なお，v, b の i 成分をそれぞれ vi , bi , K の (i, j) 成分を ki j と記述する．

2·4

拘束条件

拘束ベクトル ci が接触抗力に対応する場合， fi , vi は以下の条件を全て満たす必要がある．

fi ≥ 0(6)vi ≥ 0(7)fi = 0 or vi = 0(8)条件 (6) は抗力が物体同士を反発させる方向にしか働かないことに対応し，条件 (7) は物体同士がめり込まないことに対応する．また条件 (8) は抗力が物体同士のめり込みを防ぐのに必要なだけ働くことに対応する．

(9)を満たせばよい．しかし，摩擦力の上限は抗力に依存するので，この接触に対応する抗力が fz cz であるとす
ると，摩擦係数 µ に関して，

qfx2 + fy2 ≤ µ fz(10)を条件 (9) に優先して満たす必要がある．
拘束ベクトル ci が剛体間の接続に対応する場合，力は双方向に働くことが可能で，その大きさにも制限がない．従って，拘束条件としては (9) と同様に，vi = 0のみを満たせばよい．
条件 (10) を除けば，上に挙げた条件を満たす拘束力は Linear Complementarity Problem (LCP) [9] の一形態である Mixed LCP (MLCP) として解くことができる．
MLCP は，式 (5) のような線形式に関して，変数の各成分が以下のいずれか満たすような解を求める問題である．

li < fi < hi ,vi = 0(11)f i = li ,vi ≥ 0(12)fi = hi ,vi ≤ 0(13)li と hi は fi に対して設定された最小値・最大値である．
抗力の条件 (6)〜(8) に対しては，li = 0, hi = ∞ と設定すればよい．これは一般的な LCP の定義と一致する．
また，摩擦力の条件を (9) に限れば，摩擦力に対しては
li = −∞, hi = ∞ と設定すればよい．接続拘束に対しても同様に li = −∞, hi = ∞ とすればよい．以下では MLCPと一般的な LCP をまとめて LCP とする．
問題は，条件 (10) を摩擦力に含めた場合に各拘束力をどのようにして解くかである．

3. 拘束力の収束計算
条件 (10) を考慮した解法として，摩擦円錐を多角錐で近似した上で補助変数を追加し，LCP の枠組み内で解く手法が提案されている [5]．ただしこの手法では，多角錐化に伴う摩擦拘束の増加と補助変数の追加のため，LCP の次数が増えてしまい，これに伴って計算が遅くなってしまうという問題がある．また，この手法が前提とする LCP 計算アルゴリズムであるピボット法
は，実際にシミュレーションで発生する冗長性の高い拘束条件に対して数値計算上安定である実装を構築することが難しい．
本節では，LCP の収束計算アルゴリズムである Projected Gauss-Seidel 法を拡張して条件 (10) も考慮した解を得る手法を解説する．この手法は，• １接触点につき摩擦も含め３変数のみで計算可能• 多角錐近似でない摩擦円錐を実現可能• 実装の容易性と数値計算上の安定性を同時に実現という特徴がある．

3·1 Gauss-Seidel 法
まず，基本となる Gauss-Seidel (GS) 法について説明する．これは線形方程式を収束計算で解く手法のひとつである．GS 法を用いて，式 (5) において v = 0 とした方程式
Kf + b = 0(14)Fig.2 A simulation of boxes falling on the slope.の解 f を計算する場合の手順は，以下のようになる．
初めに，収束計算における f の初期値 f (0) を

f (0) = 0(15)for i = 1 to m do(16)Ã!
i−1m1(l+1)
(l+1)
(l)
(17)
fi
←−bi + ∑ ki, j f j+ ∑ ki, j f jki,ij=i+1
j=1

Fig.3 A simulation of a closed-loop link mechanism
700
500
400
300
200
100
0-100

この更新を f (l) と f (l+1) の相対誤差が一定値以下になるまで繰り返すことで，式 (14) の近似解が得られる．

3·2

Projected Gauss-Seidel 法GS 法に Projection 操作を加えることにより，LCP の解を計算することができる．この手法を Projected GaussSeidel (PGS) 法という．
Projection 操作は，「変数がそれに対して設定された最小値／最大値の範囲を超えていたら，最小値／最大値まで値を戻す」というものである．すなわち，(17) の処理に続けて以下の Projection 操作を適用する．
(l+1)

fi

¡

(l+1)

← max min ( fi, hi ) , li

¢

(18)

hi ,li は fi に対して設定された最小値，最大値である．
この処理により，f は各 fi が (11)〜(13) の条件を満たした解へと収束する．通常行列 K は全ての対角要素が正である対称行列であり，PGS 法の適用結果が LCPの解に収束するための条件を満たしている．
従って，条件 (10) を無視すれば，PGS 法によって各拘束力を計算することが可能である．
3·3

Projection 操作の拡張による摩擦条件への対応

条件 (10) を適用するには，摩擦力に対して，±∞ ではなく，最大摩擦力に従った値による Projection 操作を行えばよい．この場合，LCP では定数であるべき最大値・最小値が，抗力などの他の変数に依存して変化する値となってしまうが，PGS 法では更新ループごとにこの値を設定しなおすことにより対応可能である．
ある接触点に関して，抗力と摩擦力２軸に対応する変数がそれぞれ fz , fx , fy (z < x < y) であるとする．摩
擦力をまとめたベクトル(l+1)

fx,y(l+1)= ( fx(l+1) Tfy)

(19)に対して，条件¯¯¯ (l+1) ¯(l+1)¯ fx,y ¯ > µ fz

(20)

actualsimulation600
vertical force [N]などと設定する．
収束計算の l 回目における f の値を f (l) ，その i 要(l)素を fi とすると，f (l) を f (l+1) へ更新する手順は以下のようになる．

01

2

3time [s]456Fig.4 The vertical floor reaction force acting on the leftfoot during a stepping motionが成り立つときは，以下の Projection 操作を適用する．
(l+1)

(l+1)

fx,yµ fz
(l+1)
¯ fx,y← ¯¯(l+1) ¯¯ fx,y ¯(21)

この操作により，摩擦力 ( fx fy )T が条件 (9) に則した方向を保ちつつ，条件 (10) を満たす大きさへと収束することが期待できる．

4. 実験
本論文で述べた手法を OpenHRP の動力学計算サーバとして実装し，シミュレーションを通して手法の検証を行った．なお，順動力学計算には ABA を用い，4次の Runge-Kutta 法による積分を行った．また，シミュレーションの時間刻みは 1[msec]，拘束力を求める収束計算の繰り返し回数は 120 を上限とした．
Fig.2 は複数の箱状の剛体を落下させたシミュレーションの例であり、本論文の手法が複数物体の同時接触を安定にシミュレーション可能なことを示している．
Fig.3 は閉リンク構造のシミュレーション例である．
この機構のモデルはもともと開リンク機構として与えられており，赤い部分と青い部分の間に２軸分の接続拘束を設定することで，閉リンクを実現している．
また，HRP-2 の足踏み動作を実機とシミュレーションで比較した．Fig.4 はその際に足首力センサによって計測された鉛直床反力の比較である．この結果から，シミュレーションと実機の挙動がよく一致していることが確認できる．また，シミュレーションのグラフにおいてもノイズ状の軌跡がほとんどみられないことから，本手法の安定性を確認できる．
なお，HRP-2 のシミュレーションにおいては，実機の足内部に組み込まれた衝撃吸収ゴムブッシュ機構の /+*   !"#%$&'.
,()

Fig.5 Virtual joints for simulating shock absorber bushes
inserted below the ankles of HRP-2

Fig.6 Comparison between the simulation and the actual
motion when the stabilization control was disabled

computation time [msec]

43.5

computation time of each time frame
average time

32.5
2
1.5
10.5
005

101520
number of contact points

2530

Fig.7 Computation time of the dynamics calculation in an
integration time step.

学計算（接触力・順動力学・積分の計算とし，接触点検出や描画，コントローラなどの処理時間は含まない）にかかる時間を，接触点の数に基づいて示したものである．計測に用いた PC の CPU は Pentium D 940 (デュアルコア，3.2MHz) である．これによると，仮想ゴムブッシュ関節を含めて 36 自由度の HRP-2 に対して，数十点程度までであれば実時間に近い時間で計算出来ている．従って，ヒューマノイドの多くの動作に対して実用に耐える速度でのシミュレーションが可能であると言える．
以上の実験により，複数物体の同時接触を伴う多自由度の剛体関節機構のシミュレーションにおいて，本論文の手法が安定性，正確性，効率性の面で十分実用的であることが確認できた．

謝辞
挙動を反映するため，Fig.5 に示す足部関節モデルを用いた．このモデルでは，足首関節の下に３軸 (鉛直・本研究は、文部科学省の科学技術振興調整費によるピッチ・ロール）の仮想関節を設定し，各軸に実機の「科学技術連携施策群の効果的・効率的な推進」の一環ゴムブッシュに相当するバネ・ダンパ力を与えている． として実施したものである．ここに感謝の意を表する．
OpenHRP では従来，足裏と床との接触にバネ・ダンパ
参考文献
モデルを用いることでこのゴムブッシュの挙動をシミュ[1] H. Hirukawa, F. Kanehiro, S. Kajita, K. Fujiwara, K. Yokoi,
レーションしていた [1]．ロボットが環境と多様な接触
K. Kaneko and K. Harada: “Experimental evaluation of theを伴う動作においてより正確なシミュレーションを実
dynamic simulation of biped walking of humanoid robots”,
現するにあたっては，今回実現した正確な剛体接触とICRA2003, Taipei, Taiwan, pp. 1640–1645 (2003).
仮想ゴムブッシュ関節との組み合わせが有効であると[2] K. Yamane and Y. Nakamura: “Stable penalty-based modelof frictional contacts”, ICRA2006, Orlando, Florida, pp.
考えられる．
1904–1909 (2006).
次に，シミュレーションの大域的な妥当性を確認す[3] D. Baraff: “Fast contact force computation for nonpenetratるため，旋回・後退も含む 19 歩からなる 20 秒の歩行
ing rigid bodies”, Proceedings of SIGGRAPH 94, pp. 23–34
パターン（ZMP は安定領域内に維持）の PD 制御によ(1994).
る再生を行った．まず仮想ゴムブッシュ関節なしでシ[4] 藤本, 河村：“床との衝突および摩擦を考慮した２足歩行ロボットの三次元運動シミュレーション”, 日本ロボット学
ミュレーションしたとろ，目標 ZMP に基づくフィード
会誌, 15, 6, pp. 857–863 (1997).
バック安定化制御なしでも転倒せずに動作を完了した．
[5] D. E. Stewart: “Rigid-body dynamics with friction and imこれは，剛体としての挙動を高精度でシミュレーショ
pact”, SIAM REVIEW, 42, 1, pp. 3–39 (2000).
ンできていることを示している．また，仮想ゴムブッ[6] E. Kokkevis: “Practical physicas for articulated characters”,
シュ関節を有効にしてシミュレーションしたところ，安Game Developers Conference 2004 (2004).
[7] J.-R. Chardonnet, et al.: “Dynamic simulator for humanoids
定化制御が無い時は転倒し，安定化制御を有効にする
using constraint-based method with static friction”, IEEE Inと転倒せずに動作を完了した．これらは実機と同じ挙ternational Conference on Robotics and Biomimetics, Kun動である．Fig.6 は安定化制御が無い時の動作を実機とming, China, pp. 1366–1371 (2006).
シミュレーションで比較したものであり，転倒に至る[8] R. Featherstone: “Robot Dynamics Algorithms”, Kluwer
動作が概ね実機と同様にシミュレーションできている
Academic Publishers (1987).
[9] K. G. Murty: “Linear Complementality, Linear and Nonlinことが確認できる．
ear Programming”, Helderman-Verlag (1998). http://www最後に，本手法による計算時間を示す．Fig.7 は上述
personal.umich.edu/˜murty/.の HRP-2 の動作シミュレーションにおいて，１回の力