䣔䣕䣌䢴䢲䢲䢺䣃䣅䢳䣋䢳䢯䢲䢳

重心ヤコビアンを用いたリンク機構の動力学計算○園田隆(九工大) 石井和男(九工大)

磯部大吾郎(筑波大)Dynamics Calculation of Link Mechanisms Employing COG Jacobian*Takashi SONODA (KIT), Kazuo ISII (KIT) and Daigoro ISOBE (University of Tukuba)
Abstract －In this paper, we show a computational method that is employing Jacobian connecting
center of gravity (COG) of links with active joints. This Jacobian usually connects COG of thewhole-body with active joints and it is used for motion planning and controls of humanoids. Thismethod is deriving COG Jacobian that connects COG of each link with active joints, and calculating
the inverse dynamics by using the principle of virtual work.
Key Words: COG Jacobian, link mechanisms, dynamics.

1.

緒言

ロボットのダイナミクスを知ることは，制御や設計開発において重要なことである．しかし，解析対象が複雑化するにつれて，計算量の増加や式の煩雑化が問題となってくる．そのため，対象の構造によらない統一的な表現が望まれており，行列形式による並列計算可能な表現方法などが提案されている[1]．
提案する手法では，各リンクの重心と能動関節との関係を表す重心ヤコビアンをもちいて，機構の運動方程式をシンプルな形式で表現している．本論分では，重心ヤコビアンを用いたリンク機構の運動方
程式を仮想仕事の原理より導く．そして，本手法を用いた逆動力学シミュレーションについて示す．

iFi-1
i-1.
pGi-1.

Fig. 1 は，運動しているときの閉リンク機構の各リンク重心回りに働く力と能動対偶で発生すべきトルクの関係を示している．本論文では，簡単のために非ホロノミックな機構については言及しない．
閉リンク機構のリンク数を N，能動対偶の数を Mとする．このとき，リンクi ( i = 1,L, N )の重心の座
T
T T, θGi] とおき，全てのリンク重心座標に標を qi = [ pGiついてまとめて q = [q1T ,L, q TN ]T とする．全ての能動対偶の座標 θm ∈ R M は機構の運動を一意に決定するので，重心座標 q は θm の関数 g (θ ) を用いて表されるとする．
q = g (θm )linki

linki-1

Gi-1

k+1.
kGroundFig. 1k: Passive joint: Active joint

k-1

Model of closed-link mechanism逆動力学計算法

2.1 重心ヤコビアンの導出と加速度関係

i+1

GiG ≡ G (θ m ) =

2.

Fi.
p.Gi

∂q∂θ m(3)となり，リンク重心の速度が得られる．このときの(3)式がリンク機構の重心ヤコビアンである．さらに(2)式を微分して，重心の加速度関係を求めれば，
q&& = Gθ&&m + G& θ&m(4)となる．以上のように，重心ヤコビアンは重心の速度と能動対偶の速度との関係を表す行列である．
2.2 重心ヤコビアンを用いた逆動力学

前節で求めた重心加速度 q&& によって発生する慣性力と能動対偶で加えるべきトルク τ m ∈ R M は仮想仕事の原理を用いれば次式で表せる．

(1)∂r T F = 0(5)∂r = [∂q T , ∂hT , ∂θ mT ]T(6)

F = [−( Mq&& + b)T ,− f T , τ mT ]T(7)

このとき，(1)式を時間微分すると，q& = Gθ&m(2)

ここで， ∂q , ∂h 及び ∂θm はそれぞれリンク重心，➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛䣔䣕䣌䢴䢲䢲䢺䣃䣅䢳䣋䢳䢯䢲䢳
任意位置及び能動関節の仮想変位，b は能動対偶に起因しない重力等のリンク重心に加わる外力，f は任意位置 h に加わる外力，M は各リンクの質量と慣性モーメントもしくは慣性テンソルを対角に配置した質量マトリクスである．(5)式を展開して整理すれば，

2

link1Tmm

3(10)m⎡ F1 ⎤⎢F ⎥⎢ 2⎥⎢⎣ F3 ⎥⎦

141

1

1: Passive joint⎡G1 (θ 1 , θ 3 ) ⎤⎡τ 1 ⎤ ⎢⎥⎢τ ⎥ = ⎢G 2 (θ 1 , θ 3 )⎥⎣ 3 ⎦ ⎢G (θ , θ ) ⎥⎣ 3 1 3 ⎦T⎡ F1 ⎤⎢F ⎥⎢ 2⎥⎢⎣ F3 ⎥⎦F3q3q23link34

4: Active joint(12)

ここで，(a)および(b)での各関節変位 θi (i = 1,L,4)は閉リンクの拘束条件を満足しているものとする．
両方の機構とも目標軌道を Fig.3 に示す波形で与えたときの逆動力学シミュレーションの結果を Fig.4に示す．

4.

結言

リンク重心と能動対偶との速度関係を表す重心ヤコビアンを用いたリンク機構の逆動力学計算について示した．また，仮想仕事の原理から重心ヤコビアンを用いた運動方程式の導出し，行列表現により能動関節のトルクが陽に表せることを示した．
[1] 磯部大吾郎:“有限要素法を用いたリンク機構の逆動力学計算”, 日本ロボット学会誌, Vol.20, No.6,
pp.647-653, 2002.

314Fig. 3Target motion

Fig. 4Torque curves(11)(b) では能動対偶は θm ≡ [θ1 , θ3 ]T であり， (a) と同様に能動対偶のトルクは次式より得られる．

➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛33(b) Model of a mechanism with aredundant actuator.

2

重心ヤコビアンを用いた閉リンク機構の逆動力学シミュレーションを示す．
Fig.2 は単純な 4 節リンク機構のモデル図である．
(a)は駆動冗長性を持たない機構，(b)は駆動冗長性を持つ機構で，共に同じ形状・質量の部材を使用している．また，リンクはすべて同じものである．
(a) では能動対偶は θ m ≡ θ3 であり，そのトルクは重心ヤコビアンを用いて次式より得られる．
Tq1

F1
41

逆動力学シミュレーション⎡G1 (θ 3 ) ⎤[τ 3 ] = ⎢⎢G 2 (θ 3 )⎥⎥⎢⎣G 3 (θ 3 ) ⎥⎦link1link31(9)

ここで， (10)式は機構の一般的な運動方程式の形式であり， H (θ m ) は慣性行列， B (θ m , θ& m ) は重力，遠心力やコリオリ力などをまとめた項となる．以
上のように，重心ヤコビアンを用いることでリンク機構の運動方程式をシンプルな形式で表現することが可能である．

3.

q23link2Fig. 2 Two types of link mechanisms for the dynamic analysis examples
employing the COG Jacobian matrices.となる．エラー! 参照元が見つかりません。式は，リンク機構の逆動力学が重心ヤコビアンを用いて解けることを示している．ここで，J は重心以外の任意位置に加わる外力に関するヤコビアンである．たとえば，ロボットアームにおいて任意座標 h を手先位置に取れば，手先に加わる外力と釣り合うトルクを計算することが可能である．ただし，機構に配置拘束や運動拘束を含む場合には，それらの拘束条件を満たす仮想変位でなければならない．次に(4)式をエラー! 参照元が見つかりません。式に代入すれば次式が得られる．

mF3

q3

F22(a)
Model of a mechanism
without redundant actuators.

Tτ m = G T MGθ&&m + G T MG& θ&m + G T b + J T f= H (θ )θ&& + B (θ , θ& )33(8)⎛ ∂h ⎞⎛ ∂q ⎞⎟⎟ f⎟⎟ ( Mq&& + b) + ⎜⎜τ m = ⎜⎜⎝ ∂θm ⎠⎝ ∂θm ⎠≡ G T ( Mq&& + b) + J T f

23link2q1

F11− ∂q T ( Mq&& + b) − ∂hT f + ∂θ mT τ m = 0

F22