多関節三叉ヘビ型移動ロボットの 解析と制御

多関節三叉ヘビ型移動ロボットの
解析と制御
東京大学 石川将人
[email protected]
2003年5月28日 第3回制御部門大会
第3回制御部門大会 – p.1/27
発表概要
1. 背景
2. 1 リンクモデル
可制御構造の解析
回転制御・推進制御
3. 2 リンクモデル
可制御構造の解析
回転制御・推進制御
4. まとめ
第3回制御部門大会 – p.2/27
発表概要
1. 背景
2. 1 リンクモデル
可制御構造の解析
回転制御・推進制御
3. 2 リンクモデル
可制御構造の解析
回転制御・推進制御
4. まとめ
第3回制御部門大会 – p.2/27
ヘビ型移動ロボット
駆動関節
受動車輪
(Nonholonomic constraints)
広瀬ら (1972 –): Active Cord Mechanism
Titech COE/Super-Mechano Systems (1997–2001)
Serpenoid 曲線に基づく推進制御 (蛇行の原理)
受動車輪が非ホロノミックな拘束を受けるシステム
運動学モデルは 2 入力の対称アフィンシステム
第3回制御部門大会 – p.3/27
可制御性とジェネレータ
ヘビ型ロボットの運動学を状態方程式で表すと…
対称アフィンシステム
ξ̇ = g1 (ξ)u1 + g2 (ξ)u2 ,
ξ ∈ Rn , gi (x) : Rn → Rn
Ḡ = { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · }
dim Ḡ = n となるので可制御
2 ジェネレータ・高次の可制御構造を持つ
構造が複雑になるほど難しい・面白い
第3回制御部門大会 – p.4/27
対称アフィン系の分類
[石川’01 非ホロノミック系の位相構造]
対称アフィン系の分類と典型的な Example は…
1gen.・1 次 : { g1 , g2 , [g1 , g2 ]}
⇒ 車両
1gen.・高次 : { g1 , g2 , [g1 , g2 ]}, [g1 , [g1 , g2 ]], [g1 , [g1 , [g1 , g2 ]]], · · · }
⇒ トレーラ
2gen.・1 次 : { g1 , g2 , g3 , [g1 , g2 ], [g1 , g3 ], [g2 , g3 ]}
⇒ [浦久保ほか] 宇宙ロボット
2gen.・高次 : { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · }
⇒ ヘビ
第3回制御部門大会 – p.5/27
対称アフィン系の分類
[石川’01 非ホロノミック系の位相構造]
対称アフィン系の分類と典型的な Example は…
1gen.・1 次 : { g1 , g2 , [g1 , g2 ]}
⇒ 車両
1gen.・高次 : { g1 , g2 , [g1 , g2 ]}, [g1 , [g1 , g2 ]], [g1 , [g1 , [g1 , g2 ]]], · · · }
⇒ トレーラ
2gen.・1 次 : { g1 , g2 , g3 , [g1 , g2 ], [g1 , g3 ], [g2 , g3 ]}
⇒ 三叉ヘビ
2gen.・高次 : { g1 , g2 , [g1 , g2 ], [g1 , [g1 , g2 ]], [g2 , [g2 , g1 ]], · · · }
⇒ ヘビ
2gen・1 次の構造を持つ移動ロボットを考えていたら思いついた
第3回制御部門大会 – p.5/27
これまでの研究
[岩谷・石川・原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (ある種の正準形) に近似し,時間軸状態
制御形を拡張した切り替えフィードバック制御
第3回制御部門大会 – p.6/27
これまでの研究
[岩谷・石川・原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (ある種の正準形) に近似し,時間軸状態
制御形を拡張した切り替えフィードバック制御
問題点
第3回制御部門大会 – p.6/27
これまでの研究
[岩谷・石川・原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (ある種の正準形) に近似し,時間軸状態
制御形を拡張した切り替えフィードバック制御
問題点
近似の有効範囲が狭い
⇒ 微調整にはよいが移動には向かない
第3回制御部門大会 – p.6/27
これまでの研究
[岩谷・石川・原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (ある種の正準形) に近似し,時間軸状態
制御形を拡張した切り替えフィードバック制御
問題点
近似の有効範囲が狭い
⇒ 微調整にはよいが移動には向かない
移動原理がよく分からない
Serpenoid のようなシンプルな移動原理はないのだろうか?
第3回制御部門大会 – p.6/27
これまでの研究
[岩谷・石川・原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (ある種の正準形) に近似し,時間軸状態
制御形を拡張した切り替えフィードバック制御
問題点
近似の有効範囲が狭い
⇒ 微調整にはよいが移動には向かない
移動原理がよく分からない
Serpenoid のようなシンプルな移動原理はないのだろうか?
特異点に引っ掛かりやすい
第3回制御部門大会 – p.6/27
アウトライン
1. 背景
2. 1 リンクモデル
可制御構造の解析
回転制御・推進制御
3. 2 リンクモデル
可制御構造の解析
回転制御・推進制御
4. まとめ
第3回制御部門大会 – p.7/27
1 リンクの場合(運動学モデル)
各車輪は滑らない
NT
FRO
ベースの配置 w = (x, y, θ0 )T
関節角ベクトル φ
Ā(φ)RθT0 ẇ
= B̄(φ)φ̇
Ā(φ), Rθ0 は正則, B̄(φ) = I
θ0
P (x, y)
1
1
長さは適当に正規化
第3回制御部門大会 – p.8/27
1 リンクの場合(運動学モデル)
各車輪は滑らない
NT
FRO
ベースの配置 w = (x, y, θ0 )T
関節角ベクトル φ
Ā(φ)RθT0 ẇ
= B̄(φ)φ̇
Ā(φ), Rθ0 は正則, B̄(φ) = I
θ0
P (x, y)
1
1
長さは適当に正規化
本来の (物理的な) 入力は φ̇ だが、入力変換を前提として
v = RθT0 ẇ を制御入力と考える
先にベースの運動を決め、逆運動学を解いて関節角速度を決定
第3回制御部門大会 – p.8/27
1 リンクの場合(状態方程式)
d
dt
"
φ
w
#
=
"
Ā(φ)
Rθ 0
#
v
第3回制御部門大会 – p.9/27
1 リンクの場合(状態方程式)

Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1



 Sφ2 +α2 − Cφ2 +α2 − 1 − Cφ2 

" # 

S
−
C
−
1
−
C
d φ
 φ +α
φ3 +α3
φ3 
= 3 3
v

 Cθ0
dt w
−S
0
θ
0




Cθ0
0

 Sθ 0
0
0
1
第3回制御部門大会 – p.9/27
1 リンクの場合(状態方程式)

Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1



 Sφ2 +α2 − Cφ2 +α2 − 1 − Cφ2 

" # 

S
−
C
−
1
−
C
d φ
 φ +α
φ3 +α3
φ3 
= 3 3
v

 Cθ0
dt w
−S
0
θ
0




Cθ0
0

 Sθ 0
0
0
1
ξ̇ = g1 (ξ)v1 + g2 (ξ)v2 + g3 (ξ)v3
第3回制御部門大会 – p.9/27
1 リンク: 可制御構造の解析
Ḡ = span{g1 , g2 , g3 , [g1 , g2 ], [g2 , g3 ], [g3 , g1 ]}

= R6 (可制御)
Sφ1 +α1 − Cφ1 +α1 − 1 − Cφ1 1 Sα1 + Sφ1 +α1 −Cα1 − Cφ1 +α1

S
 φ2 +α2 − Cφ2 +α2 − 1 − Cφ2

S
 φ3 +α3 − Cφ3 +α3 − 1 − Cφ3

 Cθ
−Sθ0
0
0


 Sθ
Cθ 0
0
0

0
0
1
1 Sα2 + Sφ2 +α2
1 Sα3 + Sφ3 +α3
0
Cθ 0
0
Sθ 0
0
0


−Cα2 − Cφ2 +α2 


−Cα3 − Cφ3 +α3 



−Sθ0



Cθ 0

0
第3回制御部門大会 – p.10/27
1 リンク: 回転制御
θ0 だけを変化させたい。

 √
√
3
1
−2 1 − 3 1
− 2
2


 0
−1 −2 1
0
− 2

 √
√

 3
1
−
2
1
3
1


2
Ḡ(0) = span  2


 1
0
0
0
1
0




0
1
0
0
0
1


0
0
1 0
0
0
第3回制御部門大会 – p.11/27
1 リンク: 回転制御
θ0 だけを変化させたい。

 √
√
3
1
−2 1 − 3 1
− 2
2


 0
−1 −2 1
0
− 2

 √
√

 3
1
−
2
1
3
1


2
Ḡ(0) = span  2


 1
0
0
0
1
0




0
1
0
0
0
1


0
0
1 0
0
0
[g1 , g2 ] + 2g3 = ∆θ0
!!
初めに [g1 , g2 ] の方向に動かし、次いで g3 の方向に動かせば良い。
第3回制御部門大会 – p.11/27
ホロノミー
Lie 括弧積 [g1 , g2 ] の効果

−Aω sin ωt



v(t) =  Aω cos ωt 
0
第3回制御部門大会 – p.12/27
ホロノミー
Lie 括弧積 [g1 , g2 ] の効果

−Aω sin ωt



v(t) =  Aω cos ωt 
0
第3回制御部門大会 – p.12/27
1 リンク: 回転制御の手順
h
i
1. 入力 v(t) = −Aω sin ωt, Aω cos ωt, 0 を何周期か加える。
Step 1
h
2. 入力 v(t) = 0 0 1
iT
を加えて関節角 φ を 0 まで戻す。
戻す
Step 2
第3回制御部門大会 – p.13/27
1 リンク: 回転制御 (simulation)
Powered by MATX & Gtk+
第3回制御部門大会 – p.14/27
1 リンク: 推進制御
x 座標だけを変化させたい。

 √
√
3
1
− 2
−2 1 − 3 1
2


 0
−1 −2 1
0
− 2

 √
√

 3
1
−
2
1
3
1


2
Ḡ(0) = span  2


 1
0
0
0
1
0




0
1
0
0
0
1


0
0
1 0
0
0
第3回制御部門大会 – p.15/27
1 リンク: 推進制御
x 座標だけを変化させたい。

 √
√
3
1
− 2
−2 1 − 3 1
2


 0
−1 −2 1
0
− 2

 √
√

 3
1
−
2
1
3
1


2
Ḡ(0) = span  2


 1
0
0
0
1
0




0
1
0
0
0
1


0
0
1 0
0
0
−[g2 , g3 ] + 2g1 = ∆x
!!
-[g2 , g3 ] の方向に動かし、次いで g1 の方向に動かせば良い。
第3回制御部門大会 – p.15/27
1 リンク: 推進制御の手順
h
i
1. 入力 v(t) = 0, −Aω sin ωt, Aω cos ωt を何周期か加える。
Step 1
h
2. 入力 v(t) = 1 0 0
iT
を加えて関節角 φ を 0 まで戻す。
戻す
戻す
Step 2
第3回制御部門大会 – p.16/27
1 リンク: 推進制御 (simulation)
第3回制御部門大会 – p.17/27
これまでの研究
[岩谷,石川,原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (双線形) に近似し,時間軸状態制御系を
適用した切り替えフィードバック制御
問題点
近似の有効範囲が狭い
⇒ 微調整にはよいが移動には向かない
移動原理がよく分からない
Serpenoid のようなシンプルな移動原理はないのだろうか?
第3回制御部門大会 – p.18/27
これまでの研究
[岩谷,石川,原 理論シンポ’01]
1 リンクの三叉ヘビのモデルを提案
First-order system (双線形) に近似し,時間軸状態制御系を
適用した切り替えフィードバック制御
問題点
近似の有効範囲が狭い
⇒ 微調整にはよいが移動には向かない
移動原理がよく分からない
Serpenoid のようなシンプルな移動原理はないのだろうか?
特異点に引っ掛かりやすい
第3回制御部門大会 – p.18/27
アウトライン
1. 背景
2. 1 リンクモデル
可制御構造の解析
回転制御・推進制御
3. 2 リンクモデル
可制御構造の解析
回転制御・推進制御
4. まとめ
第3回制御部門大会 – p.19/27
2 リンクの場合(運動学モデル)
φ22
Ā(φ)RθT0 ẇ = B̄(φ)φ̇
w ∈ R3 ,
φ32
φ31
φ21
φ ∈ R6
φ12
φ11
B̄(φ), Rθ0 は正則, Ā(φ) は列フルランク
各関節の角速度には拘束が存在し、実質 3 自由度
v = RθT0 ẇ を制御入力と考える
先にベースの運動を決め、逆運動学を解いて得られる関節角速
度が拘束を満たす入力である
第3回制御部門大会 – p.20/27
2 リンクの場合(状態方程式)
d
dt
"
φ
w
#
=
"
B̄(φ)−1 Ā(φ)
Rθ 0
#
v
第3回制御部門大会 – p.21/27
2 リンクの場合(状態方程式)

1
S1+α
1
C1+α

1
1
 −S 1 − S 1
C
+
C
1+α
1−2+α
1+α
1−2+α


2
2

S
C
1+α
1+α

  
2
2
2
2
 −S1+α
−
S
C
+
C
1−2+α
1+α
1−2+α

d φ 
3
3
=
S1+α
C1+α

dt w

3
3
3
3
− S1−2+α
C1+α
+ C1−2+α
 −S1+α


C θ0
−Sθ0



C θ0
Sθ 0

0
0
ξ̇ = g1 (ξ)v1 + g2 (ξ)v2 + g3 (ξ)v3
−1 − C11



+


2 
−1 − C1 

2
2
C1 + C1−2 

3 
−1 − C1  v


3
3
C1 + C1−2 


0



0

1
C11
1
C1−2
第3回制御部門大会 – p.21/27
2 リンク: 可制御構造の解析
Ḡ = span{g1 , g2 , g3 ,[g1 , g2 ], [g2 , g3 ], [g3 , g1 ],
[g1 , [g1 , g2 ]], [g2 , [g1 , g2 ]], [g1 , [g3 , g1 ]]}
= R9
第3回制御部門大会 – p.22/27
2 リンク: 可制御構造の解析
Ḡ = span{g1 , g2 , g3 ,[g1 , g2 ], [g2 , g3 ], [g3 , g1 ],
[g1 , [g1 , g2 ]], [g2 , [g1 , g2 ]], [g1 , [g3 , g1 ]]}
= R9
messy
第3回制御部門大会 – p.22/27
2 リンク: 回転制御

θ0 だけを変化させたい
φ∗r = ( π2 , β, π2 , β, π2 , β),
(ただし β ; 127◦ ) のとき
[g1 , g2 ] − g3 = ∆θ0
!!

1


 −0.8 




 1 




 −0.8 





g3 =  1 
,


 −0.8 




 0 




 0 


1


1


 −0.8 




 1 




 −0.8 





[g1 , g2 ] =  1 



 −0.8 




 0 




 0 


0
[g1 , g2 ] の方向に動かし、次いで -g3 の方向に動かせば良い。
第3回制御部門大会 – p.23/27
2 リンク: 回転制御 (simulation)
第3回制御部門大会 – p.24/27
2 リンク: 推進制御
x だけを変化させたい
φ∗t = (0, − 23 π, 0, 0, 0, 23 π)
のとき
−[g2 , g3 ] + g1 = ∆x
!!
 √ 
− 3
 √ 
 3 




 0 




 0 


 √ 
,
g1 = 
3


 √ 
− 3




 2 




 0 


0
 √ 
− 3
 √ 
 3 




 0 




 0 


 √ 

[g2 , g3 ] = 
3


 √ 
− 3




 1 




 0 


0
-[g2 , g3 ] の方向に動かし、次いで g1 の方向に動かせば良い。
第3回制御部門大会 – p.25/27
2 リンク: 推進制御 (simulation)
第3回制御部門大会 – p.26/27
まとめ
三叉ヘビ型移動ロボット (1 リンク・2 リンク) について,可制
御性構造を解析し、周期制御入力による回転・推進制御方法を
提案した。特に 1 リンクの場合の Locomotion の原理はほぼ明
らかになった。
今後の展開
動力学モデルへの拡張
実機製作
第3回制御部門大会 – p.27/27