䢴䣃䢳䢯䣋䢲䢷

重心ヤコビアンを用いた閉リンク機構の逆動力学計算
Inverse Dynamics Calculation for Closed Link Mechanisms Employing COG Jacobian○学 園田 隆（九工大） 正正 磯部大吾郎（筑波大）

石井 和男（九工大）

Takashi SONODA, Kyushu Institute of Technology
Kazuo ISHII, Kyushu Institute of Technology
Daigoro ISOBE, Tsukuba University
Closed-link mechanisms are known to be usually ones that are more complex than open-link mechanisms.
Therefore, a scheme that is applied to closed-link mechanisms is actively researched. In this paper, we show acomputational method that is employing Jacobian connecting center of gravity (COG) of links with active joints. This
Jacobian usually connects COG of the whole-body with active joints and it is used for motion planning and controlsof humanoids. This method is deriving COG Jacobian that connects COG of each link with active joints, andcalculating the inverse dynamics by using the principle of virtual work.
Key Words: Closed-link mechanisms, Inverse dynamics, COG Jacobian.

1. 緒言
動力学問題は機械やロボットの設計、制御や動作計画にしばしば利用されている。多くの産業ロボットではスカラ型のマニピュレータを採用しており、効率的な動力学計算法であるニュートン・オイラー法によって高速に解を求めることが可能である。しかし、閉ループ構造を持つ機械やロボットも少なからず存在し、一般的にその動力学解を求めるのは困難である。このような機構に対して適用可能な動力学計算法は以前から研究の対象となっている。
中村らは、閉リンク機構を任意の関節で切断し、一旦木構造開リンク機構に置き換え、ニュートン・オイラー法などでトルクを計算した後、閉リンク機構の拘束条件式から得たヤコビアンを用いて機構間のトルクを変換する方法を提案している[1]。この方法では、一旦すべての関節に能動関節があると仮定するために計算過程で変数が増加するなどの問題がある。
一方で、著者らは、リンク重心に加わる力と能動関節のトルクとの関係を表すヤコビアンを用いることで、関節トルクを陽に表せる手法を開発した。このヤコビアンは、ヒューマノイドの動作計画などに用いられ、重心ヤコビアンと呼ばれている[2]。これまで、重心ヤコビアンはロボット全体の重心を表すものとして用いられていたが、本手法では各リンクに対して重心ヤコビアンを求めている。そのため、各リンク重心回りに働く力が各能動対偶にどの程度影響しているかを式として表すことが可能である。また、重心ヤコビアンは各リンク重心の速度・加速度を求める際に導出でき、さらに機構の慣性行列などは重心ヤコビアンで記述できるため、効率的に機構の運動方程式を導出できる。
本論文では、この重心ヤコビアンを用いた閉リンク機構の逆動力学計算について述べる。

2. 逆動力学
2.1 重心ヤコビアンの導出と加速度関係
Fig. 1 は、運動しているときの閉リンク機構の各リンク重心回りに働く力と能動対偶で発生すべきトルクの関係を示している。本論文では、簡単のために非ホロノミックな機構については扱わない。ただし、非ホロノミックな機構への拡張は容易である。
閉リンク機構のリンク数を N、能動対偶の数を M とする。

Fi-1i-1.
pGi-1.

iFi.
p.Gi

Gi

i+1
linki

linki-1

Gi-1

k+1.
k: Passive joint: Active joint

k-1
GroundFig. 1kModel of closed-link mechanism

こ の と き 、 リンクi ( i = 1,L, N ) の 重 心 の 座 標 を
TT T
qi = [ pGi, θGi] とおくと、能動対偶の座標 θm ∈ R M は機構の運動を一意に決定するので、重心座標 q i は次式となる。

qi = g i (θ m )(1)

q& i = G i (θ m )θ& m(2)∂gi∂θm(3)(1)式を微分すると、Gi (θm ) =となり、重心の速度が得られる。このときの(3)式がリンクi の重心ヤコビアンである。さらに(2)式を微分して、重心の加速度関係を求めると次式となる。

q&&i = G i (θ m )θ&&m + G& i (θ m )θ& m(4)以上のように、重心の速度・加速度を計算する過程で重心ヤコビアンが導出される。

䣝䣐䣱䢰䢢䢲䢺䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢺䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣐䣣䣩䣣䣰䣱䢮䢢䣌䣣䣲䣣䣰䢮䢢䣌䣷䣰䣧䢢䢷䢯䢹䢮䢢䢴䢲䢲䢺䢴䣃䢳䢯䣋䢲䢷䢪䢳䢫

2.2 運動方程式の導出&&i を用いてニュートン・オイラー
前節で求めた重心加速度 qの運動方程式より各リンクの重心に加えるべき力 Fi は、2link1として求められる。ここで、M i はリンクi の質量マトリクス、b(q i , q& i ) は重力やその他抗力をまとめた項である。
つぎに、リンクi の重心に加えるべき力 Fi とこの力に釣り合うすべての能動関節のトルク τ mi ∈ R M との関係式は、(3)、(5)
式と仮想仕事の原理を用いて次式のような関係が得られる。

τ mi = GiT Fi

1(5)

1

link3

link1q3q1

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

3

link23F3F221q2333link3F3q344: Active joint(b) Model of a mechanism with aredundant actuator.

Fig. 2 Two types of link mechanisms for the dynamic analysis
examples employing the COG Jacobian matrices.

(6)τ m = G T MGθ&&m + G T MG& θ& m + G T b= H (θ )θ&& + B (θ , θ& )mq2

233: Passive jointm32

上式は、機構内の能動関節に対して Fi がどの程度影響しているかを表している。そのため、リンクi と独立した部分は影響を受けないためにトルクがゼロになる。
(6)式をすべてのリンクに対してまとめて整理すると、次式で表せる。

m13

link2q1F1Fi = M i q&&i + b(q i , q& i )

F2

2

14(7)

mFig. 3

Target motion

Fig. 4Torque curves

ここで、 τ m , G , M および b は、全てのリンクに対してまとめた能動関節のトルク、重心ヤコビアン、質量マトリクスおよび b(q i , q& i ) である。また、(7)式は機構の運動方程式であり、H (θ m ) は慣性行列、 B (θ m , θ& m ) は重力、遠心力やコリオリ力などをまとめた項となっている。

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

4. 結言T⎡ G1 (θ 3 ) ⎤ ⎡ F1 ⎤[τ 3 ] = ⎢⎢G 2 (θ 3 )⎥⎥ ⎢⎢F2 ⎥⎥(8)⎢⎣G 3 (θ 3 ) ⎥⎦ ⎢⎣ F3 ⎥⎦(b)では能動関節は θ m = (θ1 , θ3 ) であり、そのトルクは次式より得られる。

⎡ G1 (θ 1 , θ 3 ) ⎤⎡τ 1 ⎤ ⎢⎥⎢τ ⎥ = ⎢G 2 (θ 1 , θ 3 )⎥⎣ 3 ⎦ ⎢G (θ , θ ) ⎥⎣ 3 1 3 ⎦T

⎡ F1 ⎤⎢F ⎥⎢ 2⎥⎢⎣ F3 ⎥⎦

重心ヤコビアンを用いた逆動力学計算手法を示した。本手法
は、リンク重心回りの力との釣り合いを考えているため、ニュートン･オイラー法との対比から理解が得やすく、閉リンク機構の動力学を効率よく計算する方法として期待できる。また、重心ヤコビアンを用いることで効率的に運動方程式を導出できることを示した。この利点を活かすことで、順動力学や制御への応用が考えられる。
さらに、本手法は閉リンク機構だけでなく、開リンク機構への応用も可能であるため、リンク機構の統一的な解法としても期待できる。

(9)

文
[1]

目標軌道を両方の機構とも Fig.3 で与えたときの逆動力学シミュレーションの結果を Fig.4 に示す。

[2]

献中村仁彦, "パラレルメカニズムの動力学" ，日本ロボット学会誌,vol.10, no.6, pp.709-714, 1992.
杉原知道,中村仁彦, "非駆動自由度の陰表現を含んだ重心ヤコビアンによる逆型ロボットの全身協調反力操作" ，日本ロボット学会誌, vol.24 no.2 pp.222-231, 2006.

䣝䣐䣱䢰䢢䢲䢺䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢺䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣐䣣䣩䣣䣰䣱䢮䢢䣌䣣䣲䣣䣰䢮䢢䣌䣷䣰䣧䢢䢷䢯䢹䢮䢢䢴䢲䢲䢺䢴䣃䢳䢯䣋䢲䢷䢪䢴䢫