-
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
2つの姿勢
原点中心,半径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) 運動試験‥各関節を固定し,各軸単独で重力補償+入力を与える ( が測定可なら,最小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
運動方程式
に関して線形
が測定可なら,いくつかの時刻で測定を行い,最小2乗法などを用いて一挙に推定
パラメータ
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 âモデルの不正確さやパラメータ変動に対処
オンライン推定
-
(補)適応制御
セルフチューニングコントローラ
モデル規範型適応制御
制御対象コントローラ
設計機構 同定機構
目標値 出力
制御対象コントローラ
規範モデル 適応機構
出力
目標値