䢴䣒䢳䢯䣌䢲䢳

柔軟リンク系の弾性変形を補償する動作計画手法の開発
Development of Motion Planning Scheme for Compensating
Elastic Deformation of Flexible Link Systems○非 藤井 浩平（筑波大院）

正磯部 大吾郎（筑波大）

Kohei FUJII, Graduate school, Univ. of Tsukuba, s1220906@u.tsukuba.ac.jp
Daigoro ISOBE, Univ. of Tsukuba, isobe@kz.tsukuba.ac.jp
In recent years, robot mechanisms are made lighter than before to improve the safety of robots in human society andto decrease the running cost. Elastic deformation in the structural members normally occurs when high-speed operations or
heavy-load operations are performed. The deformation may interfere with the operation. In this study, we developed a motion
planning scheme to predict and compensate the elastic deformation of flexible link systems and the effect is estimated.
Key Words: Elastic deformation, Flexible link system, Motion planning1.

M u 緒言

m t  t

近年のロボット工学の発展に伴い，例えば人と接するロボットには人との接触を踏まえ安全性を考慮した軽量化や柔軟化の観点から，また宇宙用マニピュレータには，打ち上げ時のコスト削減や作業効率を向上させるための高速化の観点から機構の軽量化が図られている．しかし，機構の軽量化に伴い，高速動作や高負荷な作業を行う際に慣性力による弾性たわみが発生するため，部材剛性を正確に考慮し，弾性たわみを抑制または補償する必要性が増すものと考えられる．具体的には，リンク部材が大きくたわむ場合，目標位置と実際のリンク先端位置との間に差が生じるため，タスク達成が困難となる．
また，リンクの動作軌道近辺に障害物が存在した場合，弾性たわみの影響で障害物と接触してしまう恐れがある．例えば，宇宙空間でロボットアームにより目的の場所へ物体を搬送し結合する作業を行う際，弾性たわみによる位置ずれが発生するため，アームの位置・姿勢を目標位置に対し補正する操作が求められる[1]．
本研究では，リンクを動作させる際に生じる弾性たわみをフィードフォワード的に予測し，軌道上でそのたわみ量を補償する動作計画手法を開発した．弾性たわみの算出には有限要素法を用いた．本報告では，簡単な柔軟リンク系に対して実施した数値例を紹介する．

2. 部材の弾性変形を補償する動作計画手法
本章では，マニピュレータの動作計画手法を構築する上で重要となる，軌道計算アルゴリズムおよびステップ毎のたわみ補正角度の算出方法について説明する．

2.1 部材剛性を考慮した軌道計算
柔軟リンク系の軌道計算を行うためには，部材に作用する慣性力によって生じるたわみ量を計算する必要がある．本研究では，有限要素法を柔軟リンク系の運動学計算に適用させた軌道計算アルゴリズム[2]を用いて，軌道計算を行う．
弾性たわみと内力の発生が無視できる剛体リンク系では，時刻t  t における増分型運動方程式として次式を用いる．

M u m t  t

 Ft t(1)ここで， M  は質量マトリクス， F t  t は時刻 t  t における外力ベクトル，um t t は動作加速度ベクトルである．一方，弾性たわみと内力の発生を考慮する必要があるリンク系の場合，次式により表される時刻 t  t における増分型運動方程式を使用する．

 M ud t t  C u d t t K u   Fdt  t

 Rt(2)ここで， C  は減衰マトリクス， K  は剛性マトリクス， Rt は要素内力ベクトル，ud t  t はリンクの変形による節点加速度ベクトル， ud t  t はリンクの変形による節点速度ベクトル， u d  はリンクの変形量増分ベクトルをそれぞれ示している．式(2)に Newmarkの β 法を適用し，動作量と変形量を足し合わせることにより，動作量，変形量，総変位量は次式のように求められる．

ud t t  ud t  ud um t t  um t  um ut t  um t t  ud t t

(3a)
(3b)
(3c)

um t t を目標軌道として式(2)の運動方程式に入力し解くことで逐次的に u d  が算出され，最終的に動作量ベクトルum と変形量ベクトルud の和(式(3c))として慣性と減衰，剛性を考慮した軌道が得られる．

2.2 たわみ補正角度の算出
図 1 に 1 関節柔軟リンク系，図 2 に多関節柔軟リンク系におけるたわみ補正角度の概念を示す．図内のステップ毎の補正角度 θnは次式に示すベクトル計算により算出する．
 n  cos1 a n  b n a n  b n (4)ここで，an はステップ毎の柔軟リンク系のリンク根元位置から先端位置へのベクトル，bn はステップ毎の剛体軌道上のリンク根元位置から先端位置へのベクトル，wn はリンク先端のたわみ量，w’nは補正有りのリンク先端位置と剛体軌道間の距離を示す．変数右下の添え字 n は対象とするリンクの番号，x, y, z は要素座標系の各軸方向成分，X, Y, Z は全体座標系の各軸方向成分であることを示す．
たわみ補正角度を算出する際には，まず前節で示した運動学計
算アルゴリズムに剛体リンク系の軌道を入力し，動作に伴う弾性変形を考慮した軌道を得る．次に，変形量ベクトルud からリンク先端のたわみ量 wn が分かるため，この値からベクトル an およびbn を導出し，式(4)からステップ毎の補正角度を算出する．この補正角度を当該ステップにおける剛体リンク系目標軌道の関節角に加え，先端位置の補正を行う．また，図 2 のような多関節柔軟リンク系の場合には，リンク毎にたわみ補正角度を求める．

䣐䣱䢰䢢䢳䢵䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢵䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣵䣷䣭䣷䣤䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢴䢯䢴䢷䢮䢢䢴䢲䢳䢵䢴䣒䢳䢯䣌䢲䢳䢪䢳䢫

ていると判断できる．図 3 (a)では水平になるにつれ，たわみ量が大きくなるため，剛体リンク系の軸上からの差は開いていく．これに対し図 3 (b)では，常に弾性たわみを考慮して関節角にたわみ補正角度を追加しているため，先端位置が補正されている．図 4(a)は補正無しの柔軟リンク系の先端位置と剛体軌道の差，図 4(b)は補正有りの柔軟リンク系の先端位置と剛体軌道の差を示している．

xYzJ1yX

Elemental coordinateb1

1

Tip position after compensation

0w1'a1

Initial trajectory(Tip position of rigid body)Z-coordinate [m]w1

Tip position of flexible body

0Z-coordinate [m]

Z-0.2

-0.4

-0.2

-0.4

Fig.1 Compensation angle for one-joint flexible link system

Zrigidflexible

-0.6
0bn

wn

b1 J2

1Jn

w1 1   2 a2

YLinknz

Torque [N*m]b2yElemental coordinate

0.2
0.4
X-coordinate [m]0.6-10-20

w2

a1-30X

J10Size[m] of each link (Case 1)

0.0060×0.060×0.60

Size[m] of each link (Case 2)

0.0060×0.060×0.30

Density [kg/m3 ]2.70×103

Poisson’s ratio

0.345

Young’s modulus [GPa]70.3

Tip mass [kg]0.50, 1.0, 3.0, 5.0

00-0.01-0.01

w'1 [m]Aluminum

w1 [m]

Material

-0.02

0.5kg
1.0kg
3.0kg
5.0kg

-0.03
3

3.

2

3

Time [s]Table 1 Parameters of a link

Mass

1(c)Torque curves
Fig.3 Motion trajectories and torque curves(Case 1, Tip mass: 5.0 [kg])Fig.2 Compensation angles for multi-joint flexible link system

Link0(b)With compensation
Without compensation
With compensation

0x n1   n

Link1

0.6

(a)Without compensation

anLink2

0.2
0.4
X-coordinate [m]rigidflexible

-0.6

-0.02
0.5kg
1.0kg
3.0kg
5.0kg

-0.03

2.5
2
Rotational angle [rad.]32.52
Rotational angle [rad.](a)Without compensation(b)With compensation
Fig.4 Difference from the rigid body trajectory

動作計画の例

本章では，簡単な柔軟リンク系に対し，弾性変形の補償を導入した動作計画を実施した場合の効果について検証を行う．

Table 2 Difference from the final position of rigid body (Case 1)
3.1 解析条件
解析対象としたリンクの仕様を表 1 に示す．リンクには，関節モータの質量として 0.58 [kg]を付加している．下記に示す，異なる 2 つの条件下でのタスクにおいて，弾性変形を考慮した動作計画の検証を行った．
Case 1：1 関節柔軟リンク系で行う．弾性変形の影響を見やすいように，リンクを鉛直下向きの状態から水平面と平行になるまで 3.0秒間で π/2 [rad]振り上げる．
Case 2：上記と同じ動作を 2 関節柔軟リンク系で行う．

3.2

Tip
mass

With compensation

w1 [m]

w’1 [m]0.50 [kg]-3.37×10-3

1.10×10-5

1.0 [kg]-5.44×10-3

1.60×10-5

3.0 [kg]-1.38×10-2

3.00×10-5

5.0 [kg]-2.22×10-2

4.30×10-5

Table 3 Compensation rates (Case 1)

解析結果

Case 1 で先端質量を 5.0 [kg]とした場合の結果を図 3 に示す．なお，図 3 (a)は補正無しの場合の動作軌道，図 3 (b)は補正有りの場合の動作軌道，図 3 (c)はそれぞれ得られた軌道から算出したトルク曲線を示す．剛体リンク系の軸上に柔軟リンク系の先端位置が追従していれば，そのステップにおける弾性たわみの補償ができ

Without
compensation

End position [%]Average [%]0.50 [kg]

99.98

95.87

Tip

1.0 [kg]

99.75

96.03

mass

3.0 [kg]

98.84

96.26

5.0 [kg]

97.97

95.16

䣐䣱䢰䢢䢳䢵䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢵䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣵䣷䣭䣷䣤䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢴䢯䢴䢷䢮䢢䢴䢲䢳䢵䢴䣒䢳䢯䣌䢲䢳䢪䢴䢫0

Table 4 Difference from the final position of rigid body (Case 2)0Z-coordinate [m]Z-coordinate [m]Without
-0.2

-0.4

-0.2

rigid
flexible00.2
0.4
X-coordinate [m]-3-3.18×10-3

w’1 [m]w’2 [m]-5

1.10×10-50.50 [kg]-1.04×10

1.80×10

1.0 [kg]-1.69×10-3-5.26×10-3

5.70×10-5-4.30×10-5

3.0 [kg]-4.36×10-3-2-1.55×10-4-9.84×10-4

5.0 [kg]-7.09×10-3-2.20×10-2

1.13×10-4-8.18×10-4-0.4

Tip-0.6

With compensationcompensation
w1 [m]
w2 [m]

rigid
flexible

-0.6

0.6

0(a)Without compensation

0.2
0.4
X-coordinate [m]mass

0.6-1.36×10(b)With compensation
Table 5 Compensation rates (Case 2)

Without compensation (Joint1)
Without compensation (Joint2)
With compensation (Joint1)
With compensation (Joint2)

End position [%]Torque [N*m]0Average [%]Joint1

Joint2

Joint1

Joint20.50 [kg]

99.75

99.97

97.42

93.85

Tip

1.0 [kg]

99.44

98.66

97.50

94.98

mass

3.0 [kg]

97.50

91.41

98.13

96.20

5.0 [kg]

98.31

94.08

97.35

95.30

-10-20-30
012

3Time [s](c)Torque curves
Fig.5 Motion trajectories and torque curves(Case 2, Tip mass: 5.0 [kg])0

w'1 [m]0

w1 [m]-0.005-0.005

0.5kg
1.0kg
3.0kg
5.0kg

-0.01
30.5kg
1.0kg
3.0kg
5.0kg

-0.01

2.52
Rotational angle [rad.]32.5
2
Rotational angle [rad.](a)Without compensation (Joint1) (b)With compensation (Joint1)
0-0.01-0.01

w2 [m]

w'2 [m]0-0.02

0.5kg
1.0kg
3.0kg
5.0kg

-0.03
3

2.5
2
Rotational angle [rad.]

-0.02
0.5kg
1.0kg
3.0kg
5.0kg

-0.03
34.

2.5
2
Rotational angle [rad.](c)Without compensation (Joint2) (d)With compensation (Joint2)
Fig.6 Difference from the rigid body trajectory
図 4(a), (b)からも，補正有りの場合は先端位置が補正されていることが確認できる．これを次式に表す先端位置の補正率を求める式に適用して，補正率の計算を行う．これにより，剛体リンク系の軸上に柔軟リンク系の先端位置が追従しているか確認を行った．
Compensation rate [%] =(B-A)/A×100が補正角度分だけ多く動作させるため，若干大きな値を示すことが確認できる．
また，2 関節柔軟リンク系にモデルを拡張した Case 2 について，先端質量を 5.0 [kg]とした場合の結果を図 5 に示す．図 5 (a)は補正無しの場合の動作軌道，図 5 (b)は補正有りの場合の動作軌道，図5 (c)はそれぞれ得られた軌道から算出したトルク曲線を示す．
Case 1 の場合と同様に，図 5(a)の補正無しの場合と比べ，図 5(b)の補正有りの場合は各リンクの先端位置が補正されていることが分かる．図 6(a), (c)は補正無しの場合のリンク先端位置と剛体軌道の差，図 6(b), (d)は補正有りのリンク先端位置と剛体軌道の差を示すが，これらの結果からも補正有りの場合は各リンク先端位置が補正されていることが確認できる．表 4 はタスク終了位置での各リンク先端位置と剛体軌道間の距離を示している．Case 1 と同様にタスク終了時での補正率とステップ毎の補正率の平均値を求め，その結果を表 5 に示す．タスク終了位置，ステップ毎の補正率の平均値共に 91%以上の高い補正率が得られていることから，補償できていると判断できる．図 5(c)で示すように，Case 1 と同様に補正有りでのトルク曲線の方が大きくでている．また，Case 2 ではトルク曲線に振動が現れている．図 6(b)，(d)を見ると，Link1，Link2 共に先端の目標位置との間の距離が変動しており，その影響によるものと考えられる．

(5)

ここで，A は補正無しの柔軟リンク系の先端位置と剛体軌道間の距離（w1, w2 …など）B は補正有りの柔軟リンク系の先端位置と剛体軌道間の距離（w’1, w’2 …など）をそれぞれ示している．表 2
はタスク終了位置での先端位置と剛体軌道間の距離を示している．
Case 1 におけるタスク終了時での補正率とステップ毎の補正率の平均値の算出結果を表 3 に示す．タスク終了位置，ステップ毎の補正率の平均値共に 95%を超えていることから，補償できていると判断できる．図 3(c)に関しては，補正有りでのトルク曲線の方結言

柔軟リンクを動作させた際の部材に発生する弾性たわみの補償を行う動作計画手法を開発した．
柔軟 1 リンク系と 2 リンク系について動作計画を行った結果，補正を実行しながら先端位置を目標位置に近づけることができ，タスク終了位置でほぼ一致させることができた．タスク終了時での補正率とステップ毎の補正率の平均値共に高い値が得られた．
多リンク系の場合においても，本手法が適用できるものと考えられる．
今後は，動作経路近辺に障害物があることを想定した際に，部材変形分の補償を行い，障害物との接触を避ける動作計画を実施可能とする予定である．また，実機による検証を行う予定である．

参考文献
[1]
[2]

茂原正道，鳥山芳夫，"衛星設計入門"，培風館，2002.
磯部大吾郎，加藤昭博，"フレキシブル・リンク系のモデルベー
スド・フィードフォワード制御"，ロボット学会誌，vol.25, No.4,pp.625-631, 2007.

䣐䣱䢰䢢䢳䢵䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢵䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣵䣷䣭䣷䣤䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢴䢯䢴䢷䢮䢢䢴䢲䢳䢵䢴䣒䢳䢯䣌䢲䢳䢪䢵䢫