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

Constraint Force Calculation of Articulated Rigid Bodies using a Gauss-Seidel-like method*Shin’ichiro NAKAOKA, Shizuko HATTORI, Fumio KANEHIRO,Shuuji KAJITA and Hirohisa HIRUKAWA (AIST)
Abstract— We describe our constraint-based contact force solver where the force solution is calculated by an iterative
method based on the Projected Gauss-Seidel method. The method was verified by several simulation tests that include thecomparison between the actual and simulated motions of HRP-2, and the results show the validity of the method.

1. はじめに
近年，複雑な関節機構をもつロボットを対象として，より高度な動作の実現を目指す研究が盛んであり，そのような動作で発生する複雑な接触を安定・正確・高効率に処理できるシミュレータが求められている．
接触によって発生する力を計算する一般的な手法のひとつであるペナルティ法はシンプルかつ高速な手法
ではあるが，剛体同士の接触としては必ずしも正確ではない上，シミュレーション結果がパラメータ設定や積分手法・積分間隔に大きく依存してしまい，我々の目指すシミュレータに対しては適切な手法ではない．
剛体同士の接触を正確にシミュレーション可能な手法として，物理的な拘束条件を LCP[1] として定式化し，その解をピボット法で計算する手法 [2, 3] が提案されている．ロボットのシミュレータにこの手法を応用するにあたっては，自由度の多い関節機構に対する計算の効率化と，実際にシミュレーションで発生する多数の拘束条件に対する計算の安定化が課題となる．
山根ら [4] はピボット法を数値計算上安定に処理するアルゴリズムを開発し，これを基に上記の課題を解決している．
一方，計算の安定化に関しては，収束計算を用いるのも有効な手段である．この場合，収束計算の繰り返し回数に上限を設けることにより，解の正確さと計算速度に関するトレードオフを調整可能であるというメリットもある．本研究では，摩擦も含めた接触・拘束力の解を Gauss-Seidel 法を拡張した収束計算によって数値計算上安定に求める手法を構築し，収束計算による接触力計算の妥当性を検証する．

2. 計算手法の概要
2·1

拘束条件の設定

物体間の接触が有限個の接触点と各接触点に設定される直交３軸の拘束ベクトル（うち１軸は法線方向に設定）によって表現されるものとする．ある状態における拘束ベクトルの総数を m ∈ ℜ とし，i (≤ m) 番目の拘束ベクトルを ci ∈ ℜ3 , (|ci | = 1)，このベクトルに対応する拘束点を pi ∈ ℜ3 とする．
拘束ベクトル ci に関して作用反作用の関係にある拘束力の一方を fi ci , ( fi ∈ ℜ) ( fi は物体同士を反発させる方向を正) とし，拘束力を含めた物体にかかる力によって拘束点 pi に発生する相対加速度の ci 成分を ai ∈ ℜとすると， fi を並べたベクトル f = ( f1 f2 · · · fm )T とai を並べたベクトル a = (a1 a2 . . . am )T に対して，以下の線形式を得ることができる．

a = Af + a0(1)行列 A ∈ ℜm×m は各拘束力が各拘束点におよぼす加速度の割合を示しており，ベクトル a0 ∈ ℜm は拘束力以外の力による各拘束点の加速度を示している．
Kokkevis の手法 [5] を用いることで，式 (1) の定数要素は関節機構の自由度 n，拘束ベクトル数 m に対してO(nm + m2 ) の計算量で計算することが可能である．
式 (1) をシミュレーションにおける離散化時間でオイラー積分することによって，速度に関する以下の式を得ることができる．

v = Kf + b(2)v は積分後の拘束点 pi における相対速度の ci 成分を並べたベクトルである．
ある接触点における法線方向の拘束ベクトルを cz とすると，抗力の性質より， fz と vz は以下の条件を満たす必要がある．
fz ≥ 0(3)vz ≥ 0(4)fz = 0 or vz = 0(5)また，この接触点における残りの（接平面上の) 拘束ベクトルを cx , cy とすると，まず摩擦力が無限に働くと仮定すれば(vx vy )T = 0(6)を満たせばよい．しかし，摩擦力の上限は抗力に依存し，摩擦係数 µ に関して，q

fx2 + fy2 ≤ µ fz(7)を満たすことも考慮する必要がある．

第8回システムインテグレーション部門講演会(SI2007)（2007年12月20日 〜 22日・広島）- 909 -SY0012/07/0000 - 0909 © 2007 SICE /+700
vertical force [N]600

actualsimulation

500400*   !"#%$&'.

Fig.1 A simulation of boxes falling on the slope.

,()Fig.3 Virtual joints for simulating shock absorber bushes
inserted below the ankles of HRP-2

300
200
100
00123time [s]4546computation time [msec]-100

Fig.2 The vertical floor reaction force acting on the leftfoot during a stepping motion

2·2

解の計算

線型方程式の解を収束計算によって求める GaussSeidel(GS) 法を式 (2) に対して適用すると，v = 0 となる f の解を得ることができる．
条件 (3)〜(5) を満たした解を求めるためには，GS 法に Projection 操作を加えればよい．すなわち，f の要素fi が抗力に対応する場合，収束計算において fi を更新する際に，以下の操作も行う．

fi ← max ( fi , 0 )(8)式 (2) の行列は，この操作の結果が条件を満たした解へと収束するための条件を満たしている．
さらに，接平面上の拘束に関して摩擦の条件 (7) を適用するために，摩擦力に基づく Projection 操作を行う．ある接触点に関して，抗力と摩擦力２軸に対応する拘束力の変数をそれぞれ fz , fx , fy (z < x < y) とすると，摩擦力をまとめたベクトル fx,y = ( fx fy )T に対して，以下の Projection 操作を適用する．

fx,y ← ¯¯µ fz¯ fx,yfx,y ¯if¯¯¯ fx,y ¯ > µ fz

(9)3. 実験とまとめ
本論文で述べた手法を OpenHRP3 の動力学計算サーバとして実装し，シミュレーションを通して手法の検証を行った．なお，順動力学計算には Featherstone のArticulated Body Algorithm を用い，4 次の Runge-Kutta法による積分を行った．また，シミュレーションの時間刻みは 1[msec]，拘束力を求める収束計算の繰り返し回数は 120 を上限とした．
Fig.1 は複数の箱状の剛体を落下させたシミュレーションの例であり，本論文の手法が複数物体の同時接触を安定にシミュレーション可能なことを示している．
また，HRP-2 の足踏み動作を実機とシミュレーションで比較した．Fig.2 はその際に足首力センサによって計測された鉛直床反力の比較である．この結果から，シミュレーションと実機の挙動がよく一致していることが確認できる．また，シミュレーションのグラフにお3.5

computation time of each time frameaverage time

32.5
21.5
10.5005101520
number of contact points

2530

Fig.4 Computation time of the dynamics calculation in anintegration time step.

いてもノイズ状の軌跡がほとんどみられないことから，本手法の安定性を確認できる．
なお，HRP-2 のシミュレーションにおいては，実機の足内部に組み込まれた衝撃吸収ゴムブッシュ機構の挙動を反映するため，Fig.3 に示す足部関節モデルを用いた．このモデルでは，足首関節の下に３軸 (鉛直・ピッチ・ロール）の仮想関節を設定し，各軸に実機のゴムブッシュに相当するバネ・ダンパ力を与えている．
Fig.4 は HRP-2 の動作シミュレーションにおいて，動力学サーバ（接触力・順動力学・積分を処理）の計算時間を示したものである．これによると，仮想ゴムブッシュ関節を含めて 36 自由度の HRP-2 に対して，数十点程度の接触点であれば実時間に近い時間で計算出来ており，ヒューマノイドの多くの動作に対して実用に耐える速度でのシミュレーションが可能であると言える．
以上の実験により，複数物体の同時接触を伴う多自由度の剛体関節機構のシミュレーションにおいて，本論文の手法が安定性，正確性，効率性の面で十分実用的であることが確認できた．
参考文献[1] K. G. Murty: “Linear Complementality, Linear and Nonlinear Programming”, Helderman-Verlag (1998).
[2] D. Baraff: “Fast contact force computation for nonpenetrating rigid bodies”, Proceedings of SIGGRAPH 94, pp. 23–34(1994).
[3] D. E. Stewart: “Rigid-body dynamics with friction and impact”, SIAM REVIEW, 42, 1, pp. 3–39 (2000).
[4] 山根, 中村：“ピボット法に基づく LCP の解法とその衝突・接触シミュレーションへの応用”, 第 25 回日本ロボット学会学術講演会, 千葉 (2007).
[5] E. Kokkevis: “Practical physicas for articulated characters”,
Game Developers Conference 2004 (2004).

- 910 -