1A1-A08

LCP に基づく接触力計算を用いたヒューマノイドロボットの力学シミュレーション
Dynamics simulation of humanoid robots using contact force calculation
based on LCP○中岡慎一郎 服部静子 金広文男 梶田秀司 比留川博久 (産総研)

Shin’ichiro NAKAOKA, Shizuko HATTORI, Fumio KANEHIRO,Shuuji KAJITA and Hirohisa HIRUKAWA (AIST)
Abstract— This paper describes a LCP-based method for calculating contact forces in dynamics simulation of humanoidrobots. The method is based on the algorithm proposed by Kokkevis, and we propose additional procedures that makethe algorithm more practical in the reality and numerical stability. The simulation system is veriﬁed by experiments using
humanoid robot HRP-2, and the result shows the eﬃciency and validity of the method.
Key Words: dynamics simulation, linear complementarity problem (LCP), contact force, humanoid robot

1. はじめにヒューマノイドロボットを対象とした力学シミュレーションでは，一般的に以下の要素が求められる．

1. 剛体間の接触における挙動が正確であること
2. 関節機構としての整合性を保持できること
3. 自由度が高い関節機構に対しても効率的に計算できること
これらを実現するにあたっては，順動力学だけでなく，物体間の接触力を計算するアルゴリズムが重要となる．
接触力を仮想的なバネ・ダンパで近似する手法 [1, 2]
は一般的な接触力計算手法のひとつである．これは接触点数に対して線形時間で計算可能な効率的な手法であるものの，上述の 1 に関して厳密ではなくなる．
一方，剛体間の接触力を正確に求めるために，物理的な拘束条件を 2 次計画法として定式化する手法 [3] やLinear Complementarity Problem (LCP)[4] として定式化する手法 [5, 6] が提案されている．ただし多くの手法
は慣性行列を用いた運動方程式に基づくため，関節機構の一般化座標においては，機構の自由度 n に対してO(n3 ) の計算量が必要となり，上述の３が厳しくなる．
あるいは，関節機構を各リンクに対応する独立した剛体と各関節に対応する拘束の付加によって表現する手法もあるが [7]，この場合関節に対応する拘束力の誤差によって，２が厳密でなくなる傾向がある．
この問題に関して，Kokkevis[8] は関節機構における効率を考慮した LCP 化の手法を提案しており，これはヒューマノイドのシミュレーションへ適用することも期待できる．ただしこの提案においては，シミュレータを構築するにあたって不明確な点がいくつかある．
本論文では，Kokkevis のアルゴリズムをベースにシミュレータを構築するために必要な要素として，(1) 数値的に安定な収束計算の適用法，(2) 静止摩擦への対応，および (3) 足部ゴムブッシュの組み込みを提案する．また，HRP-2[9] を対象としたシミュレーションと実機動作の比較によって，手法の妥当性を検証する．

2. Kokkevis アルゴリズムの概要
物体間の接触が m 個の接触点と各接触点における法線で表されるとすると，各接触点の法線方向に発生する抗力の大きさを並べたベクトル fN ∈ <m と，各接触点の相対加速度の法線成分を並べたベクトル aN ∈ <mに関して，以下の線形式が成り立つ．

aN = KN fN + a0N(1)

KN と a0N は順動力学を繰り返し計算することによって求める．まず fN = 0 の状態で順動力学を計算し，各接触点に生じる相対加速度の法線成分を a0N とする．
KN の j 列目の成分は，fN の j 成分のみを 1 とした状
態で順動力学を計算した結果から a0N を引いた値とすればよい．順動力学計算に Articulated Body Algorithm(ABA) [10] を用いることにより，関節機構の自由度 n，接触点数 m に対して，O(nm) の計算量で KN と a0N を求めることができる．
接触抗力を求めるにあたっては，拘束によって強制される目標加速度 adN を設定する．例えば，非弾性衝
突や既に接触中の状態に対しては，接触点における相対速度の法線成分が 0 となるよう adN を設定する．そして，抗力は接触時にのみ単一方向に働くという性質と，物体同士は貫通しないという拘束から，以下の条件を満たす fN が解となる (以下の説明では，ベクトルの不等式条件は各要素ごとに適用されるものとする)．
aN − adN ≥ 0(2)fN ≥ 0(3)(aN − adN )T fN = 0(4)なお，このような関係を相補性 (complementarity) といい，上式は記号 ⊥ を用いて以下のようにも記述される．
aN − adN ≥ 0⊥
fN ≥ 0(5)そして，式 (1) のような線形式について相補性を満たす解を求める問題を LCP という．

[No. 07-2] Proceedings of the 2007 JSME Conference on Robotics and Mechatronics, Akita, Japan, May 10-12, 2007
1A1-A08 (1)抗力に加えて，摩擦力も考慮する必要がある．動摩擦力は接触点における相対速度の滑り成分の逆方向に働くものと仮定し，式 (1) にその成分を組み込むと，以下のように書ける．
  
 aN   KNN KT N   fN   a0 
 =  
 +  N  (6)aTKNT KT TfTa0T
ここで fT は各接触点における摩擦力の大きさを，aTは摩擦力が働く方向への加速度を表している．式 (5) と同様に，相対速度の滑り成分が 0 となるような目標加速度 adT に対して以下の条件を設定することで，摩擦力も含めた LCP を得ることができる．

aT − adT ≥ 0⊥

fT ≥ 0(7)ただしこの条件のみでは摩擦力が無限に働いてしまう．クーロン摩擦を仮定すると，摩擦係数 µ に対して，以下の条件も満たす必要がある．

fT≤µfN(8)

この条件も含めて LCP として定式化するには，変数λ を加えて以下の条件を設定すればよい．

3.

(aT − adT ) + λ ≥ 0⊥

fT ≥ 0(9)µfN − fT ≥ 0⊥

λ≥0(10)

LCP 解の収束計算

LCP を解くアルゴリズムは大きくピボット法と収束計算法とに分けられる．ピボット法は理論上は有限回のステップで正確な解が得られるものであるが，実際にはシミュレーションで発生する複雑で冗長な拘束条件に対して数値計算上安定に解を得ることは難しい．
数値計算上安定に LCP の解を得るためは，収束計算が有効である．ただし，式 (5)，(9)，(10) の条件から構築する LCP の行列は，そのままでは収束計算を適用するための条件を満たすことができない．一方，式 (6) の行列は正定値対称行列となっており，式 (5)，(6)，(7)に基づく LCP は収束計算によって近似解を求めることが可能である．ただし，この場合は摩擦力の上限が考慮されないことが問題となる．
摩擦力の上限を含めて収束計算で解くためには，条件を Mixed Complementarity Problem (MCP) として定式化すればよい．線形の MCP は，線形式z = Ax + b(11)の各成分に関して以下のいずれか満たすような解 x を求める問題である．

xi = li&zi > 0(12)xi = hi&zi < 0(13)li < xi < hi&zi = 0(14)解の下限 li と上限 hi は x の各成分 xi ごとに設定される．これは LCP のより一般的な表現であり，以下ではこの線形 MCP も含めて LCP と呼ぶこととする．

我々が解くべき問題は，式 (5)，(6)，(7) に関して，解の下限・上限を以下のように設定した LCP とすることができる．

0

≤ fN

≤

∞

(15)

0

≤ fT

≤µfN

(16)

変数 fT の上限値は変数 fN に依存しているが，収束計算においてはこの値を毎ループごとに更新していくことができる．以下に，収束計算アルゴリズムのひとつである Projected Gauss Seidel 法によってこの LCP を解く手順の例を示す．

f ←0以下繰り返し

f or i : 1 · · · m i−1
2m∑∑
1 ki, j f j +ki, j f j + bi fi0 ← −ki,ij=1()fi ← max 0, fi0

j=i+1

f or i : m + 1 · · · 2m
 i−1
2m
∑

1 ∑0 ki, j f j +fi ← −ki, j f j + bi ki,i j=1
j=i+1
(())
fi ← min µ fi−n , max 0, fi0
なお，m は接触点の数とし，この例では全ての接触点に対して動摩擦を設定できるものとする．また，ki, j ， fi ，bi はそれぞれ






 a0 + ad  fN  KNN KT N NN 
K = 
 , , b =  0
 , f = aT + adTfTKNT KT Tの各成分を表すものとする．

4. 静止摩擦への対応
ここでは，接触点における相対速度の滑り成分が 0または微小なときに働く摩擦を静止摩擦とする．静止摩擦に関しては，摩擦力の働く方向を滑り速度の方向から定めることはできないため，2. 節で述べた動摩擦の計算法では対応できない．
シミュレーションにおいては，静止摩擦を無視するのもひとつの方法ではある．この場合，本来静止し続けるべき状況においても微小な滑りを繰り返すことになるが，これは時間刻みが十分小さければ問題にならないことも多い．
静止摩擦を無視できない場合は，式 (6) の fT をひとつの接触点につき４方向にとることで対応する．図1-(a) に示すように，動摩擦においては摩擦力の働く方向をひとつに定め，この方向への力の大きさを求めた．
静止摩擦においては図 1-(b) に示すように，接触点の接平面上で直行する 2 軸をとり，fT を各軸の正負 4 方向への力の大きさに対応させる．これにより，滑りを止めるため任意の方向へ摩擦力をかけられるようになる．
ただし，この手法では可能な反力の集合が図 2-(b) に示すような四角錐になってしまい，最大静止摩擦力の[No. 07-2] Proceedings of the 2007 JSME Conference on Robotics and Mechatronics, Akita, Japan, May 10-12, 2007
1A1-A08 (2)Fig.1 Directions of the frictional constraint

Fig.2 Possible regions of reactve forces in the static state.
The left cone shows the true region, and the right quadrangular pyramid is the region in our method.

pitch moment [Nm]Fig.4 A virtual joint model of absorber bushes. Translational z-axis, pitch and roll axes are added to the parts
below the anke joints, and linear spring-damper force
to keep the original position is given to each joint.

151050-5-10-15-20-25

simulation
real machine

time

Fig.5 The pitch-axis moment acting on the left foot during
the body oscillation caused by inclining the bodyFig.3 The foot impact absorption mechanism of HRP-2[9]. The ankle and the sole are connected by threeunits of an absorber bush and a damper.

大きさが摩擦の働く方向に関して均一でなくなってしまう．これは本来図 2-(a) に示すような円錐になるのが正しい．
静止摩擦はこの問題を考慮した上で組み込む必要がある．ロボットの動作が摩擦力にある程度の余裕を見込んで計画・制御されるものであれば，これは大きな問題とはならないと思われる．また，ロボットと環境との接触は一般的に複数の接触点で表されるため，図1-(b) における基準軸を接触点ごとにランダムにとることで，最終的に働く摩擦力の方向依存性が緩和されることが期待できる．

5. 足部ゴムブッシュの組み込み
二足歩行ロボットは，歩行の安定化のため足裏や足内部に何らかの衝撃吸収構造をもつものが多い．HRP-2
では，図 3 に示すように，足首下部と足底の間が 3 点でゴムブッシュ・ダンパ機構により接続されている [9]．
この機構は歩行の安定に大きな影響を及ぼす要素であるため，シミュレーションにおいてはできる限りその挙動を忠実に再現することが望ましい．これに関して
従来の OpenHRP[11] では，足裏と床との接触力をバネダンパモデルで計算することで，ロボット全体の挙動をゴムブッシュのある実機に近づけていた [1]．
本研究では，足裏と床との接触は剛体接触として計算し，足内部にゴムブッシュに相当する仮想的な関節を組み込むことで，より実機に近いシミュレーションを実現する．
図 4 は HRP-2 のゴムブッシュ機構に対応する仮想関節のモデルである．足首関節の下部に，新たに垂直並進・ピッチ・ロールの３軸を付加してあり，各関節には関節原点位置へ復帰しようとする線形のバネダンパ力を与えるものとする．またシミュレーションの安定化のため，ゴムブッシュ機構の各軸に対して仮想的な減速器付サーボモータのロータ慣性モーメントを設定して，順動力学計算を行うものとする．

6. 評価
本論文で述べた手法を OpenHRP[11] の動力学計算サーバとして実装し，HRP-2 を対象として実機とシミュレーションで動作結果を比較した．コントローラは PD制御と目標 ZMP フィードバックによるもので，実機とシミュレーションで同一のプログラムを用いた．なお，順動力学計算には ABA を用い，4 次の Runge-Kutta 法による積分を行った．また，シミュレーションの時間刻みは 0.001[s]，収束計算の繰り返し回数の上限を 120とした．4. 節で述べた静止摩擦への対応は滑り速度が1.0 × 10−3 [m/s] 以下である場合に有効とした．
まず，ゴムブッシュの基本特性を比較するため，直立姿勢の HRP-2 を床上で足首ピッチ軸まわりに傾けることで発生する胴体の減衰振動を観測した．図 5 はこの際の足首力センサのピッチ軸出力を示しており，シミュレーションは振動の初期において概ね実機と同様の周期と減衰率を再現していることが分かる．なお，ゴ[No. 07-2] Proceedings of the 2007 JSME Conference on Robotics and Mechatronics, Akita, Japan, May 10-12, 2007
1A1-A08 (3)0.008700
500

computation time [s]vertical force [N]

0.007

actualsimulation

600
400
300
200
100
0-100
012

3
time [s]456

computation time of each step
average time

0.006
0.005
0.004
0.003
0.002
0.001
00Fig.6 The vertical ﬂoor reaction force acting on the leftfoot during a stepping motion

roll moment [Nm]1015202530

number of contact points

Fig.8 Computation time of the dynamics calculation. The
points show the correlation between the numer of contact points and computation time in a time step.

25205actualsimulation

157. まとめ1050-5-100

123time [s]456Fig.7 The roll-axis ﬂoor reaction moment acting on the leftfoot during a stepping motion

ムブッシュのバネ定数は HRP-2 の仕様値を用いており，ダンパ定数については本実験を通して調整したものとなっている．
次に，同じ場所で左右交互に足踏みを行う動作を行わせた際の床反力を足首力センサで計測した．図 6 と
図 7 はそれぞれ力の鉛直成分およびロール軸に関するモーメントを表しており，シミュレーションと実機の挙動がよく一致していることが確認できる．また，グラフにおいてノイズ状の軌跡がほとんどみられないことから，本手法の安定性を確認できる．
さらに，旋回・後退も含む 19 歩からなる 20 秒の歩行パターン（ZMP は安定領域内に維持）に関して，

1. ゴムブッシュなし・安定化制御なし
2. ゴムブッシュあり・安定化制御なし
3. ゴムブッシュあり・安定化制御ありの３つのシミュレーションを行った．結果として，1 と3 は転倒せずに動作を完了し，2 は 2 歩めで転倒に至った．1 の結果から，剛体としての挙動を高精度でシミュレーションできていることが確認できる．また，2 と 3の結果は実機の挙動と同じである．以上により，シミュレーションの大域的な妥当性を確認することができた．
最後に，本手法による計算時間を示す．図 8 は上述の HRP-2 の動作シミュレーションにおいて，１回の力学計算（接触力・順動力学・積分の計算とし，接触点検出や描画，コントローラなどの処理時間は含まない）にかかる時間を，接触点の数に基づいて示したものである．計測に用いた PC の CPU は Pentium D 940 (デュアルコア，3.2MHz) である．これによると，仮想ゴムブッシュ関節を含めて 36 自由度の HRP-2 に対して，数十点程度までであれば実時間に近い時間で計算出来ている．従って，ヒューマノイドの多くの動作に対して十分高速なシミュレーションが可能であると言える．

実験により，本論文で示した拘束条件の LCP への定式化と収束計算による解法が，力学的に妥当な接触力を算出することが示された．また，足部ゴムブッシュのシミュレーション手法も，実機に近い挙動を得るにあたって有効であることが確認できた．我々が構築したシミュレータは，「はじめに」で述べた 3 つの要素に関して十分実用的であると言えよう．
その一方で，本手法では LCP を解く際に，接触点の数 m に対して O(m2 ) の計算量が必要となるため，多数の身体部位を環境に接する必要のある動作においてはパフォーマンスが落ちてしまうという問題がある．このような接触点の数に対するスケーラビリティを改善することが今後の課題である．
参考文献[1] H. Hirukawa, F. Kanehiro, S. Kajita, K. Fujiwara, K. Yokoi,
K. Kaneko and K. Harada: “Experimental evaluation of thedynamic 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] 藤本, 河村：“床との衝突および摩擦を考慮した２足歩行ロボットの三次元運動シミュレーション”, 日本ロボット学会誌, 15, 6, pp. 857–863 (1997).
[4] K. G. Murty: “Linear Complementality, Linear and Nonlinear Programming”, Helderman-Verlag (1998). http://wwwpersonal.umich.edu/˜murty/.
[5] D. Baraﬀ: “Fast contact force computation for nonpenetrating rigid bodies”, Proceedings of SIGGRAPH 94, pp. 23–34(1994).
[6] D. E. Stewart: “Rigid-body dynamics with friction and impact”, SIAM REVIEW, 42, 1, pp. 3–39 (2000).
[7] K. Erleben, J. Sporring, K. Henriksen and H. Dohlmann:“Physics-Based Animation”, Charles River Media, Inc(2005).
[8] E. Kokkevis: “Practical physicas for articulated characters”,
Game Developers Conference 2004 (2004).
[9] 五十棲, 赤地, 平田, 金子, 梶田, 比留川：“ヒューマノイドロボット HRP-2 の開発”, 日本ロボット学会誌, 22, 8, pp.
1004–1012 (2004).
[10] R. Featherstone: “Robot Dynamics Algorithms”, Kluwer
Academic Publishers (1987).
[11] 金広, 藤原, 梶田, 横井, 金子, 比留川, 中村, 山根：“ヒューマノイドロボットソフトウェアプラットフォーム OpenHRP”,
日本ロボット学会誌, 21, 7, pp. 785–793 (2003).

[No. 07-2] Proceedings of the 2007 JSME Conference on Robotics and Mechatronics, Akita, Japan, May 10-12, 2007
1A1-A08 (4)