2DOF SCARA Robotθi 関節 i の回転角度

mi リンク i の質量

Ii リンク i の重心回りの慣性モーメント

li リンク i の長さ

di 関節 i からリンク i の重心までの長さ

Di 関節 i の粘性摩擦係数

l1 θ

x

y

m1θ

d1

l2d2

m2I2

I1

リンク1

リンク2

関節1

関節2

x0

y0

x1y1

y2x2

1

2

手先位置Tyx ][p T][ 21 q 関節変数

DH表現リンクパラメータ 1 2 1 2 1 1 2 20, 0, ,d d a l a l

順運動学(Direct Kinematics)

ただし，

110000100

00

110000100

00

10000100

00

12

2

2

122111212

122111212

2

2

2

2222

2222

1111

1111

0

0

0

zyx

SlSlCSClClSC

zyx

SlCSClSC

SlCSClSC

zyx

)cos(),sin(cos,sin,cos,sin

21122112

22221111

CSCSCS

手先座標

12211

12211

SlSlyClClx

ヤコビ行列

12212211

12212211)(ClClClSlSlSl

J q

qqpqfp )(),( J

座標変換

特異姿勢(Singular Configuration)

望みの手先速度を出せない

,00)(det 2221 SllJ q

12212211

12212211)(ClClClSlSlSl

J q

)(rankmax)(rank qqq

JJ

qqpqfp )(),( J

x

y

逆運動学(Inverse Kinematics)

関節角

21

222

211

2

1

22

22111

1

2cos

2costan

llLll

LllLl

xy

手先座標Tyx ][p

ただし， 22 yxL x

y ),( yxT][ 21 q

２つの姿勢

原点中心，半径l1の円と，中心(x, y)，半径l2の円との交点として，求めることもできる

動力学（その1）(Dynamics)

)(),()( qgqqqhqqτ DM

2222212

22212112122)(mmCM

mCMmCMM q

2

1212

212

2212 )2(),(

SMSMqqh

2

1

00

DD

D1 1 2 122 12

( )g C g C

g C

g q

12 2 1 22 2 2

11 1 1 2 1 2 1 22

22 2 2 2

1 1 1 2 1

2 2 2

( )

( )

M m l d

m m d m l d I I

m m d Ig m d m l gg m d g

T

T

21

21

τ

q慣性行列

遠心力・コリオリ力

重力 粘性摩擦係数

ただし，

関節変数

入力トルク

動力学（その2）(Dynamics)

( ) ( , ) ( )( ) ( , ) ( )

( , , , )

M DM C D

τ q q h q q q g qq q q q q q g q

ζ q q q q a

qqqqqh

),()2(),( 2

1212

212

2212 CSM

SM

0)(

),(1212

212122212

SMSMSM

C qq

),(2)( qqq CM は歪対称行列 0),(2)( qqqqq CMT例

2 1 2 2 2 1 1 2 2 1 2 1 1 12

2 1 2 1 1 1 2 2 12

(2 ) { ( ) } 0( , , , )

0 0 0

C S C C

C S C

ζ q q q q

12 11 22 1 2 1 2TM m m D D g ga パラメータ

同定（その1）(Identification)

(1) 静止試験‥一定トルクを与え，重力と釣合せ，静止させる

(2) 運動試験‥各関節を固定し，各軸単独で重力補償＋入力を与える （ が測定可なら，最小２乗法などを用いる）, ,q q q τ

2 12 2 2 2m D

1 12 11 1 1 1(2 )M m D

(a) 関節1を固定

(b) 関節2を0°で固定

(c) 関節2を90°で固定

1 1 1 2 12

2 2 12

g C g Cg C

1 11 1 1 1m D

等角速度運動を行わせ，粘性摩擦係を求めた後，角加速度運動を行わせ，慣性行列の係数を求める方法などもある

同定（その2）(Identification)

( , , , )τ ζ q q q q a

2 1 2 2 2 1 1 2 2 1 2 1 1 12

2 1 2 1 1 1 2 2 12

(2 ) { ( ) } 0( , , , )

0 0 0

C S C C

C S C

ζ q q q q

12 11 22 1 2 1 2TM m m D D g ga

運動方程式

に関して線形

が測定可なら，いくつかの時刻で測定を行い，最小２乗法などを用いて一挙に推定

パラメータ

1ˆ ( )T Ta A A A y1 1 1 1 1

( ( ), ( ), ( ), ( )) ( ),

( ( ), ( ), ( ), ( )) ( )n n n n n

t t t t t

t t t t t

ζ q q q q τA y

ζ q q q q τ

ここで

, ,q q q

平衡点近傍での線形化(Linearization)

状態方程式 uxx BA

ただし，重力項が無い場合 （ g(q) = 0 ） は、K = 0

重力補償をする平衡点 q0 の近傍で線形化

TT 21 2121 , ux

)sin()sin()sin()}sin(sin{sin

201022201022

2010222010210121011

gdmgdmgdmdlgmgdm

K

は2×2単位行列 1 112 0,

0

JB

DJKJI

A 20 ),( IMJ q

重力補償+PDフィードバック(Gravity Compensation & PD Control)

安定性

0)()(

0)()()(21)(

qq

qqqqqqq

DKtV

KMtV

dT

dpT

dT

制御則

)()( qgqqqτ ddp KK

dp KK , 正定対角行列

また、 の時のみ0q 0)( tV

平衡点 は漸近安定)0,( dq

RobotPD

g(q)

τ+

- ++e qq ,dq

目標値関節角PTP制御

（補）Liapunovの安定判別リアプノフ関数

が連続であり，ベクトル

について恒等的に となり，

かつ，システムにそっての時間微分が

となるスカラー関数

( )Vxx

( )tx

( ) 0V x

( ) 0V x ( )V x

非線形システム の平衡点( ) ( ( ))t tx f x

リアプノフ関数が存在 リアプノフ安定

リアプノフ関数が存在して， 漸近安定( ) 0V x

0x

-

++

状態フィードバック(State Feedback)

状態が全て得られるならばオブザーバ（OBS）は必要ない

制御則

)(ˆ qgxqτ FS dD

11)( BBFACSD

Robot

OBS

g(q)

τ+ qq ,dq

F：状態フィードバックゲイン

F x̂

DS

動的補償(FF)(Inverse Dynamics Method)

誤差

制御則

),,(ˆ ddd qqqττ

（“^”は推定値を表す）

RobotID τ qq,ddd qqq ,,

)(ˆˆ),(ˆ)(ˆ),,(ˆ qgqqqhqqqqqτ DM

ffDM εeeq )(

パラメータ誤差、軌道誤差などffε

ID：逆動力学モデル

関節角追従制御目標軌道

dqqe

動的補償(FF)+PDフィードバック(Inverse Dynamics & PD Control)

制御則

eeqqqττ dpddd KK ),,(ˆRobotID τ

+-

-

+

e

qq ,

PD

ddd qqq ,, τ̂

誤差

fcpd KDKM εeeeq )()(

dqqe

動的補償(FB)+PDフィードバック(Inverse Dynamics & PD Control)

制御則

eeqqqττ dpd KK ),,(ˆRobotID τ

+-

-

+

e

qq ,

PD

ddd qqq ,, τ̂

誤差

bcpd KKM εeeeq )(

dqqe

フィードバックによる線形化とサーボ補償

線形化システム

計算トルク法(Computed Torque Method)

制御則

),,(ˆ aqqqττ RobotID τ qq

,ddd qqq ,,

誤差

ctpd KKM εeeeq ))((

eeqq pdda KK

aq

dqqe

PD+- e

-

+AG

ctpd KK εeee

順運動学

分解加速度制御(Resolved Acceleration Control)

制御則

),,(ˆ rqqqττ

誤差

rapppdp KK εeee

pppdda KK eepp

基準座標系での制御

qqpqq )()(1 JJ ar

dp ppe

RobotID τ qq,

)(1 qJ

)(qfp qqp )(J

AG

qqu )(J

-

+

u

apddd ppp ,,

pp ,

rq

スライディングモード制御(Sliding Mode Control)

制御則

, diag(

, :ˆ sgn( ), sgn( ) : )

ˆˆ ˆ ˆˆ ( ) ( , ) ( ) ( , , , )

d

d r

r d

i

r r r r

K

Λ ΛΛ Λ

K K

M C D

e q qs e e q q e q qq q eτ τ s s

τ q q q q q q g q ζ q q q q

正定対称行列

符号関数

in

iirr

TT sKgDCMtVMtV

1

)(~~),(~)(~)(,)(21)( qqqqqqqssqs

0,)(~~),(~)(~ iiirri gDCMK qqqqqqq ととれば in

ii stV

1

)(

ただし、 gggDDDCCCMMM ˆ~,ˆ~,ˆ~,ˆ~

RobotID τ

+-

-

+

s

qq ,rr qq , τ̂

)sgn( sK

ddd qqq ,, RM

チャタリング

モデルの不正確さに対するロバスト性

（VSSの一種）

1u kx のとき

yu 1x0

2x

1x0

1u kx のとき2x

1x0

0S

固有ベクトルの方向

0

0

0

0

切換え線

（補）スライディングモード制御

+

-

k

kSystem

2次系の例1 2 2 2

1

, , 0x x x ax u au x

1 2

1 2

( , ) 0( , ) 0

k x xk x x

0k 1 2 1

1 2

( , )x x x SS x x

位相面軌道

領域Ⅰ： 不安定渦状点

領域Ⅱ： 固有ベクトル方向のみ安定な鞍形点

ともに不安定であるが，ゲインの切り替えにより安定

切換え線S=0の傾きが小さいと，切替え線に到達した状態は，この直線上に拘束されて，原点へ向かう

r e yu

0

0

, :ˆ , :

ˆˆ ˆ ˆ ˆˆ ( ) ( , ) ( ) ( , , , )

d

d r

r d

D D

r r r r

Λ ΛΛ Λ

K K

M C D

e q qs e e q q e q qq q eτ τ s

τ q q q q q q g q ζ q q q q a

正定対称行列

正定対称行列

適応制御(Adaptive Control)

制御則 RobotID τ

+-

-

+

s

qq ,rr qq , τ̂

0)(,~~)(21)( 1 ssaasqs D

TTT KtVΓMtV ただし、 aaa ˆ~

調整則 ˆ ( , , , ) , :T r rΓ Γ a ζ q q q q s 正定対称行列

DK

ddd qqq ,, RM

Est âモデルの不正確さやパラメータ変動に対処

オンライン推定

（補）適応制御

セルフチューニングコントローラ

モデル規範型適応制御

制御対象コントローラ

設計機構 同定機構

目標値 出力

制御対象コントローラ

規範モデル 適応機構

出力

目標値