䢳䣃䢳䢯䣒䢲䢹

並列的解法を用いた直動リンクを有するロボット機構の逆動力学計算
Inverse Dynamics Calculation of a Robot with Linear-Motion
Link Mechanism using Parallel Solution Scheme○非 朱 栄麟（筑波大院）

正磯部 大吾郎（筑波大）

Ronglin ZHU, Graduate School, Univ. of Tsukuba, s1220925@u.tsukuba.ac.jp
Daigoro ISOBE, Univ. of Tsukuba, isobe@kz.tsukuba.ac.jp
A use of linear-motion link mechanism can effectively increase the motion range of a robot with asmall cost. However, special assumptions should be made for the expansion and contraction of thelink mechanism when the robot requires the dynamics compensation. The present report describes theassumption of these features in the inverse dynamics parallel solution scheme and an example of
torque calculation for a walking motion.
Key Words: Walk, Legged robot, Legged locomotion, Linear-motion link mechanism, Inverse dynamics

1.

[M]{u}t  t  {F}t  t

緒言

伸縮動作により効率的に可動範囲が広くなる直動リンク機
構は，ロボットの軽量化・コストダウンを図る上でも有効であり，多く使用される．一方，動力学補償を必要とする動作を行うロボットやマニピュレータでは，直動リンク機構の伸縮に伴う動力学の変化を補償する必要がある．
筆者らは，様々な形態のリンク系に対し有効な逆動力学計算法として，並列的逆動力学計算法[1](以下，並列的解法)を開発してきた．この解法は，系全体を有限要素で離散化して並列的に節点力を求め，その力学的関係より動作に必要な入力トルクを算出するが，直動リンク機構の伸縮動作に対応する機能を有していなかった．そこで本研究では，直動リンク機構の伸縮動作をモデル化し，並列的解法の適用範囲を拡張することを目的とする．直動リンクを有するロボット機構を有限要素によりモデル化し，並列的解法を適用して逆動力学計算を行った例を示す．

2

並列的解法

並列的解法では，最初に運動学計算によって算出された目
標軌道の加速度情報をもとに運動方程式を解き，系内の拘束されていない節点に作用する節点力を逐次的に算出する．

Motor 2Motor 1

(1)

ここで，[M]は全体質量マトリクス， {u} は節点加速度ベクトル，{F}は全体座標系で表される外力ベクトルである．
次に，マトリクス形式で表した次式を用いて節点力からトルクに換算する．
(2){ n }  [Ln ][T n ]{P n }n
ここで，{τ }は要素座標系における関節トルクベクトル，{Pn}は全体座標系における節点力に関するベクトル，[Tn]は全体座標系を各リンクの要素座標系に変換する座標変換マトリクス，[Ln]は部材長やリンク構造の情報を含む部材長マトリクスである．解法の詳細については文献[1]などをご覧いただきたい．

3

実験用ロボット機構

実験で用いる直動リンクを有するロボット機構の概観を図1 に示す．モータ 1 およびモータ 3 を駆動することで腕を伸縮させ，モータ 4 およびモータ 5 を使って両腕を回転させる．
このロボットについて並列的解法を用いて逆動力学計算を行う際には，図 2 に示すように 19 要素，20 節点を有するモデルとして要素分割を行った．有限要素には Bernoulli-Euler の仮定に基づく 3 次はり要素を用いた．
ロボットの腕に使用される直動リンク機構の概略図を図 3に示す．この機構では，モータの回転によりボールねじが動き，リンク 1 に対しリンク 2 が左右に移動する．

Motor 3ArmArm

1~20: Node No.
Motor 4①~⑲: Element No.

Motor 5Fig.2 Finite element subdivision of the robot

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

A

Link 2

BCΔLZY
Motor

Link 1

LX

OScrew

Fig.6 Length of an element before and after expansion

Fig.3 Linear-motion link mechanism of the arm
Link 1

Link 2
Motion1

A1, ρ1

A1+A2, ρ1+ρ2

1A2, ρ22

43

Fig.4 Finite element subdivision of the linear-motion
link mechanism (before expansion)Motion3

Motion2
Link 1

Link 2
Fig.7 Walking motion of the robot

A1, ρ1A1+A2, ρ1+ρ2A2, ρ2

110.8

Motion 3

0.6

34Fig.5 Finite element subdivision of the linear-motion
link mechanism (after expansion)

0

Motion 3

Motion 1

Motion 2

ただし，k= ΔL/L である．ΔL を 1step 当りの伸縮量とおくと，初期の ΔL が大きすぎると，動作の初期に大きな加速度が発生してしまい，振動が発生する可能性が高い．そこで，k には次式に示すように正弦補間による値を用いた．

kπ i 1  cos  2π 2 n 

ここで，n は総伸縮動作 step 数，i は動作 step 数である．

0.20Motion 1-0.2

Motion 2-0.6

直動リンク機構のモデル化および軌道算出

直動リンク機構は，2 つの異なるリンクが重なって構成されていると見なすことができる．例えば，図 4 に示す状態から図 5 に示す状態まで直動リンク機構が伸びる際，図の下側に示すように要素分割を行い，これらの状態を表現することにする．ここで，リンクの断面積を A ，密度を ρ とし，添字はリンクの番号とする．すなわち，リンクがそれぞれ独立している個所，重なっている個所に分けることが可能で，異なる断面積および密度を有する 3 つの有限要素の長さを変動させることで機構が表現できる．例えばリンクが伸びる際には，全体の総質量が変わらないように要素①と要素③を伸ばし，要素②を縮める操作を行う．
並列的解法を用いてトルクを計算する際には，各節点の時々刻々の位置（すなわち軌道）情報が必要となる．図 6 に示すように AB を伸縮前の有限要素，AC を伸縮後の有限要素とし，節点 A, B, C の位置ベクトルを a, b, c とおくと，次の関係で節点 C の位置ベクトルを求めることができる．
c  kb  a  b(3)0.4-0.4-0.5

4Torque [Nm]2Torque [Nm]10.5-0.8-1-101203

Time [s]123Time [s](a) Motor4(b) Motor5

Fig.8 Obtained torques for the motors

5

直動リンクを有するロボット機構の逆動力学計算

本研究では，ロボットの歩行動作を下記に示す 3 つの動作に分けた．動作の概要を図 7 に示す．
動作 1： 0.0 秒から 1.0 秒の間に，右腕を 63mm 伸ばす．
同時にモータ 4 を π/4 rad 回転させる．
動作 2： 1.1 秒から 2.1 秒の間に，右腕を 63mm 縮めながらモータ 4 を-π/4 rad 回転させ，胴体を前進させる．同時に左腕を 63mm 伸ばしながらモータ 5 を π/4 rad 回転させる．
動作 3：動作 2 と左右逆の動作をさせ，胴体を前進させる．
動作 2 と 3 を繰り返して連続的に歩行する．この動作の際，並列的解法を用いて算出されたモータ 4 およびモータ 5 のトルクを図 8 に示す.

6

結言

直動リンクの伸縮に対応する軌道算出アルゴリズムを構築
し，直動リンクを有するロボット機構の要素分割を行い，歩行動作における逆動力学計算を行った．今後は実際にロボットを用いて歩行制御実験を行う予定である．

(4)

7

参考文献

[1] D. Isobe: Unified Solution Scheme for Inverse Dynamics of RoboticSystems, Progress in Autonomous Robot Research (Editor: F.N. Mortensen),
Nova Science Publishers, ISBN:978-1-60456-281-1, pp.125-173, 2008.

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