ロボティクス特論

2015年度前期

第８回

動力学を用いたロボット制御のための計算

動力学シミュレーション,M.Matsuda, K.Tsujita, et al., 2008

機構自由度：１８次元制御系自由度：２４次元時間刻み：1.0 [ms]

非線形振動子とリフレックスダイナミクスを用いた生物規範型歩行制御,M.Matsuda, K.Tsujita, et al., 2008

機構自由度：１８次元制御系自由度：２４次元時間刻み：1.0 [ms]

連続体のNewtonの運動方程式

if

連続体（剛体）内の微小要素 を考える．

慣性座標系

任意点 P

(微小要素)

剛体

動座標系0r

1

1x

0

1

ix

iii fxdm

微小要素の位置座標

微小要素に作用する力

Newton の運動方程式

11 i

i

i

ii fxdm

連続体内部の質量の総和を取る

作用反作用の法則から，内力の総和はゼロになるので

FxM CM M

F

CMx

剛体に作用する外力

質量

剛体質量中心の位置ベクトル連続体の Newton の運動方程式

idm

連続体のEulerの運動方程式

if

連続体（剛体）内の微小要素 を考える．

慣性座標系

任意点 P

(微小要素)

剛体

動座標系0r

1

1x

0

1

idm

ix

iiiii fxxxdm

微小要素の位置座標

微小要素に作用する力

Euler の運動方程式

Newton の運動方程式と同様に，連続体内部の質量の総和を取る

L

N 剛体に作用する外モーメント

剛体の角運動量

連続体の Euler の運動方程式

NL

運動方程式の導出方法

ロボットの運動方程式

Newton-Euler 形式による定式化

Lagrange 形式による定式化

要素毎の力の釣り合い方程式から求める

エネルギーを基にしたスカラー関数(Lagrange 関数)の変分形式を用いて求める

基礎の復習

Newton の運動方程式

バネ質点系

質点の運動

Fxm

kxxm

02

0 xx m

k0

tctcx 0201 cossin

固有角振動数

解析力学の基礎

質点の運動

x

x

位相平面

（位相空間） A

‐A

ω0A

-ω0A

t=2π／ω0

t= 0

tctcx 0201 cossin

2

0

22

0

22

2

0

22 cossin AtAtA

xx

ばねー質点の力学系の運動は，位相平面（位相空間）内でただ１本の連続な曲線（解曲線という）となる

解析力学の基礎

系の位相空間内での運動軌道から仮想的にずれた軌道を考える＝ 仮想変分

q∈RN

位相空間

t=t1

t=t2

δq 仮想変分

始点（t=t1）と終点（t=t2）では，仮想変分を取らないとする

021

tttt

qq

解析力学の基礎

Lagrange 方程式の導出

Lagrange 関数

外力の仮想仕事

汎関数

仮想変分

UTL

qUqqTqqLL ,,

FdqW

dtWLJt

t 2

1

dtWLJt

t 2

1

Lagrange方程式

Lagrange 方程式 Fq

L

q

L

dt

d

02

1

2

1

2

1

2

1

2

1

2

1

2

1

dtqFq

L

dt

d

q

L

dtqFq

Lq

q

L

dt

dq

q

L

dtqFq

Ldtq

q

L

dtqFq

Lq

q

LJ

t

t

t

t

t

t

t

t

t

t

t

t

t

t

散逸項のあるLagrange方程式

このとき，Lagrange 方程式は次の式となる

q

VF

q

L

q

L

dt

d

系にエネルギー散逸を伴う外力が働く場合，Lagrange 方程式は，散逸項を導く逸散ポテンシャル関数から散逸力を付加することで，全体の運動方程式が求められる．

外力 F が，例えば， のような粘性抵抗の場合の

逸散ポテンシャル関数 V は，

xcF

2

2

1xcV

例）Spring-Massシステム（MDKシステム）

質量m

x

ばね定数 k

粘性係数 c

質点の運動エネルギー

バネのポテンシャルエネルギー

2

2

1xmT

2

2

1kxU

ダッシュポッドによる逸散関数2

2

1xcV

Lagrange 関数 22

2

1

2

1kxxmUTL

例）Spring-Massシステム（MDKシステム）

質量m

x

MDKシステムの運動方程式

ばね定数 k

粘性係数 c

xckxxm

x

VF

x

L

x

L

dt

d

Lagrange 方程式

xmkxxmxdt

d

x

L

dt

d

22

2

1

2

1

kxkxxmxx

L

22

2

1

2

1

xcxcxx

V

2

2

1

MDKシステムの運動

質量m

x

MDKシステムの運動方程式ばね定数 k

粘性係数 c

xckxxm tex とおいて 02 tekcm

m

mkcc

2

42

0te なので 02 m

k

m

c

特性方程式

m

k

mk

c 0,

2 　 と置いて解く

一般解は

1

2

1

1

20

200 ttt

ececex

1

1

10

特性根は２実根

特性根は重根（実根）

特性根は複素根（実部は実指数関数，虚部は三角関数）

MDKシステムの解の挙動

1

1

10

挙動は指数関数的（過減衰）

挙動は指数関数的（臨界減衰）

挙動は振動的（減衰振動）

Po

sit

ion

[m

]

Time [sec]

ζ= 1.2

ω0=1

ζ= 1.0

ζ= 0.707

ζ= 0.3

例）剛体振子

微小要素の運動エネルギー

微小要素のポテンシャルエネルギー

2222

2

1

2

1

d

l

myxd

l

mdT

cosgdl

mgyd

l

mdU

d

質量m長さ l

微小要素の位置，速度

cos

sin

y

x

sin

cos

y

x

全体の運動エネルギー2

2

0

22

62

ml

dl

mdTT

l

全体のポテンシャルエネルギー

2

coscos

0

mgld

l

mgU

l

0

LL

dt

d

Lagrange 方程式

0sin22

2

mglml

g

例）倒立振子

運動方程式

系の運動エネルギー

系のポテンシャルエネルギー

xml

mlxmMT2

cos

3

1

2

1

2

1 222

cos2

mglU

Lagrange 関数

cos22

cos

3

1

2

1

2

1 222 mglx

mlmlxmML

umlml

xmM 2

2

sin

2

cos

ux

L

x

L

dt

d

0sin22

cos

3

1 2

mgl

xml

ml 0

LL

dt

d

例）倒立振子

線形化

運動方程式

umlml

xmM 2

2

sin

2

cos

0sin22

cos

3

1 2

mgl

xml

ml

1,1,1 x 　　

uml

xmM 2

0223

1 2 mgl

xml

ml 線形化された運動方程式

ugmMl

mM

3

4両式から xを消去して

u = 0 のとき，解は双曲型の不安定系となり，

u ≠ 0 のとき，u によってθが駆動される（倒立できる可能性）

例）２リンクマニピュレータ

XO

Y

θ1

θ2

(x’,y’)

(X,Y)

第１リンク

長さ：l1 質量： m1

第２リンク

長さ：l2 質量： m2

運動方程式の導出手順（Lagrange 形式)

1. リンク上の任意点の位置ベクトルを求める

2. リンク上の任意点の速度ベクトルを求める

3. 運動エネルギー、ポテンシャルエネルギーを求める

4. Lagrange 関数を構成する

5. Lagrange 方程式を用いて運動方程式を導出する

ロボットの運動方程式の一般形

TEGHM )(),(

T

E

G

H

M

)(

),(

一般化座標慣性行列非線形項（遠心力、コリオリ力など）重力項拘束条件ヤコビ行列拘束力入力トルク