三叉ヘビ型ロボットの動力学モデリングと周期フィードバック制御
京都大学 ○石川 将人, ファナック株式会社 岩竹隆裕, 東京大学 岩谷靖，原辰次
Motion analysis and control of multi-linked trident snake robot
Masato ISHIKAWA† , Takahiro IWATAKE‡ , Yasushi IWATANI∗ and Shinji HARA∗
† Dept. of Systems Science, Kyoto University
‡ FANUC LTD.
∗ Dept. of Information Physics and Computing, The University of Tokyo
E-mail: [email protected]
Abstract: This paper is concerned with motion control of multi-linked trident snake robot, which is proposed in our recent works
as a new example of nonholonomic systems with two generators. We show that there exist some stable cycles when periodic
motion control is applied, and utilize this fact to robustify our previous result. We also extend the conventional kinematic
(velocity-input) model to dynamical (torque-input) model and propose a trajectory tracking control, where the constraint force
acting on the wheels is minimized using redundancy of the control input.
1
はじめに
三叉ヘビ型ロボットとは，従来から知られている受動車輪
型のヘビロボット [1] を中央のベースリンクを介して三叉
状に接続したもので，著者らが複雑な可制御性構造を有す
る非ホロノミックシステム [2, 3] を体現する新たな例とし
て考案したものである [4]．
著者らはまず 1 リンクの三叉ヘビに対して，文献 [5] で
提案したベキ零近似と時間軸状態制御形に基づく制御方法
を適用し，微小変位の運動に適したフィードバック制御手
法を提案した．さらに文献 [6] ではまったく異なる観点か
ら，並進または回転運動を実現する周期的（フィードフォ
ワード）入力の設計法を示した．これは入力ベクトル場の
1 階の Lie 括弧積が，単周期入力により発生する状態空間
の変位（ホロノミー）に対応するという原理に基づいた
ものである．各足が 1 リンクの三叉ヘビの場合は可制御
distribution が 1 階までの Lie 括弧積のみで張られるシステ
ム（first-order system）であるため，この手法でほぼ完全
な移動パターンを生成することに成功した．しかしなが
ら，1 リンクモデルは接地点が 3 つしかないために，実機
により実現しようとすると各車輪にかかる床面からの拘束
力が過大になり，物理的に無理がある可能性が否めない．
各足が多リンクのモデルでは拘束力が多数の車輪に分散
されるため著しい改善が期待できるが，この場合は高階の
Lie 括弧積を可制御構造に含むために制御がきわめて難し
く，いまだ「どんな状態からでも」「どこへでも」移動で
きるという段階には至っていなかった．
そこで本稿ではまず，拘束力の議論を可能にするために
2 節において従来の運動学モデルから関節トルクを入力と
した動力学モデルに拡張しておく．つづいて 3 節では，2
リンク運動学モデルを対象として先行研究の制御方法の問
題点を整理し，周期入力のもとでの安定不変サイクルを利
用することによって初期形状や誤差に対してロバストな運
動制御を実現する．4 節では，前節で生成された速度入力
に基づく軌道に追従するトルクの算出法を示し，さらにト
ルクの冗長性を利用することによって車輪にかかる拘束力
を低減できることを示す．5 節で総括を述べる．
2
2.1
は横滑りをしないものと仮定する．また，簡単のために
ベースの形状は正三角形とし，中心から各関節までの距
離を 1，各関節から車輪までの距離を 1（すなわち各リ
ンクの長さは 2）とする．2 番目の足がつながっている
頂点の方向をベースの正面とし，各頂点の方向を表す定
数を α1 = − 23 π, α2 = 0, α3 = 32 π とおく．ロボッ
トの状態はベースの配置（位置・姿勢）を表すベクトル
¡
¢T
w = x y θ0 ，およびロボットのの形状（各関節の相
対角）を表すベクトル
¡
¢T
φ := φ11 · · · φ1` φ21 · · · φ2` φ31 · · · φ3`
で 決 ま る ．こ れ ら を ま と め て 状 態 ベ ク ト ル q :=
¡ T T ¢T
で表す．
φ w
車輪が横滑りをしないという仮定から，状態変数の微分
の間には次のような Pfaff 型の非ホロノミックな拘束条件
が成立している．
Ā(φ)RθT0 ẇ = B̄(φ)φ̇
筆者らが文献 [4] で提案し，[6] で多関節に拡張した三
叉ヘビ型ロボットのモデルを Fig. 1 に示す．以下，運動
学モデルの導出までは先行研究と同じであるので詳細は
同文献を参照されたい．中央のベースリンクは 3 つの駆
動関節を持ち，ここに各足が連結されている．それぞれ
の足は駆動関節で連結された ` 個のリンクからなり，各
リンクの中点には受動車輪が取り付けられている．車輪
(2)
各行列の内訳は煩雑であるので詳述は文献 [6] に譲る．但
し Rθ0 は SE(2) の同次変換行列
0
R θ0
1
cos θ0 − sin θ0 0
@
A
sin
θ
cos
θ
0
:=
0
0
0
0
1
(3)
である．
非ホロノミック拘束 (2) はそのままシステムの運動学モ
デルを表しており，この関係式をみたすように各変数の時
間変化が決定される．
さて，運動学モデルにおける制御入力は駆動関節の角
速度 φ̇ と考えるのが自然であるが，この点については注
意が必要である．状態空間の次元が 3 + 3` であるのに対
して非ホロノミック拘束の数，すなわち (2) 式の行のサイ
ズは 3` である．したがって，系の運動学的な自由度はリ
ンク数に関わらず 3 しかなく，本来駆動関節であるはずの
3` 個の関節の角速度の間には満たさねばならない拘束が
三叉ヘビ型ロボットのモデル
非ホロノミック拘束と運動学モデリング
(1)
φ2`
φ22
φ3`
φ32
φ31
RON
φ21 F
θ0
φ1`
P (x, y)
φ12
φ11
Figure 1: Model of the Trident Snake
T
存在することを意味している．独立に指定しうる 3 つの制
御パラメータとして何を選ぶかについては任意性がある
が，次のようにちょうど 3 次元のベクトルである ẇ を指
定するのが最も物理的に理解しやすいであろう．B̄(φ) は
つねに正則な正方行列であるから，
φ̇ = Ω(q)ẇ
ただし Ω(q) := B̄(φ)−1 Ā(φ)RθT0
(4)
3n×3
R
(5)
と表すことができる．つまり，ベースの運動 ẇ を決定す
ると，非ホロノミック拘束を破らないような関節角の角速
度は一意的に決まってしまう．ベースの部分を手で掴んで
ゆっくり動かすと，足リンクは全て従属的についてくると
いう事実と整合している．
このような考え方に基づいて，本研究では運動学モデル
における制御入力を u = ẇ であると設定している．状態
方程式は次の形の対称アフィンシステムになる．
d
dt
µ
φ
w
¶
µ
=
Ω(q)
I3
¶
u
(6)
ま た ，右 辺 に 現 れ る 係 数 行 列 の 各 列 ベ ク ト ル を
g1 (q), g2 (q), g3 (q) とおく．
2.2
動力学モデルの導出
さて，本稿ではさらに進めて動力学モデルを取り扱うこ
とにする．動力学モデルにおいてはごく自然に駆動関節へ
の入力トルク τ ∈ R3` を制御入力とする．入力の自由度
が運動学モデルより多いのは，以下に示すように床面から
の反作用である拘束力によって入力の一部がキャンセルさ
れるからである．状態変数は q および q̇ となる．
導出については基本的な手順どおりであるので詳細は
省略するが，拘束条件 (2) の下での運動方程式は
µ
I3`
−ΩT (q)
¶
µ
¶
τ
O3×1
(7)
の形で表される．ここで M, C, D は質量・粘性要素からな
る物理パラメータである．λ は一般化拘束力の項であり，
系が非ホロノミック拘束 (2) を満たすように決定される．
(2) 式を時間微分すると加速度拘束
M (φ)q̈ + C(φ, φ̇) + D(φ)q̇ +
λ=
φ̈ = Ω(q)ẅ + Ω̇(q)ẇ
(8)
が得られる．λ を消去すると低次元化された運動方程式
M̂ (φ)ẅ + Ĉ(φ, φ̇) + D̂(φ)ẇ = ΩT (q)τ
(9)
が導かれる．
3
3.1
先行研究の手法とその問題点
三叉ヘビ型ロボットの可制御性は次のように保証され
る．入力ベクトル場の集合を G = {g1 , g2 , g3 } に対して 1
階の Lie 括弧積を g12 := [g1 , g2 ]，g23 := [g2 , g3 ]，g31 :=
[g3 , g1 ] とおき，2 階の Lie 括弧積を g112 := [g1 , [g1 , g2 ]]，
g212 := [g2 , [g1 , g2 ]]，g113 := 2[g1 , [g3 , g1 ]] とおく．すると
dim span{g1 , g2 , g3 , g12 , g23 , g31 , g112 , g212 , g113 } = 9
が原点近傍で成り立つ（このシステムは 2 ジェネレータの
可制御構造 [2] を持つ）．
さて，Stokes の定理によれば 1 階の Lie 括弧積 g12 は入
力 u1 , u2 による単周期入力の効果（ホロノミー）の方向
と局所的に一致する．この原理を利用して，次の２つのス
テップの反復により移動制御を実現するのが先行研究の方
針であった．
制御アルゴリズム 1.
Step 1 u1 , u2 , u3 のうち２つを組み合わせた単周期入力に
より g12 , g23 , g31 の方向の状態変位を生成する．
Step 2 定数入力により g1 , g2 , g3 の方向の状態変位を生成
する．
•
Step 2 の終了時に形状 φ が初期状態と等しく，配置 w
が所望の方向に変化するように入力を選ぶ．この方法で
は 2 階以上の Lie 括弧積を利用しておらず，g1 , g2 , g3 と
g12 , g23 , g31 の組み合わせによって生成できる変位は 6 次
元にすぎない．ゆえに 2 リンク以上の場合には任意の方向
の変位を生成することは一般には不可能である．しかしな
がらこれらのベクトル場は形状 φ に依っているので，特
定の形状においてはその可能性が残されている．
並進制御の場合は，
¡
¢T
φT = 0, 32 π, 0, 0, 0, − 32 π
(10)
を基準形状として選んでやると，φ = φT のときには g23
と g1 の線形結合によって並進成分に対応するベクトルが
張れる．すなわち，


O6×1
 cos θ0 

g23 (φ, w) + βg1 (φ, w) = k 
(11)
 sin θ0 
0
が β = −2, k = −1 について成り立つ．そこで，φ = φT
の状態から Step 1 で周期入力
¡
¢T
u = 0, −Kω sin ωt, Kω cos ωt
を与え，Step 2 で定数入力 u1 = −2K を与えて形状を
φ = φ∗t に戻すと，結果として −θ0 の方向への並進が実現
する．同様に，回転制御の場合にもある形状 φR を基準と
していた (Fig.2)．
運動学モデルにおける速度入力設計法の改良
本節ではまず先行研究の手法を整理するとともに問題点を
指摘し，続いて発見的に得られた安定な不変サイクルを利
用して，十分広い初期状態から任意の方向へ継続的に移動
できることを示す．以下では 2 リンクのモデルに限定して
議論をすすめる．
φT
φR
Figure 2: Initial shape for translation/rotation
無論この方法は完全なものではなく，以下のような２つ
の問題点があった．
• 与えられた初期形状 (もっとも典型的には φ = 0) か
ら，基準形状 φT , φR へ遷移させる方法が確立されて
いなかった．
• 周期入力によるホロノミーと Lie 括弧積の一致は周期
入力の振幅が十分小さいときに近似的に成り立つに
すぎないが，この近似誤差により，Step 2 の終了後の
φ と基準形状が正確には一致しない．
したがって，たとえ基準形状からスタートしても，反復を
重ねるごとに基準形状から解離してしまうことがあり，運
動を長く持続させることが困難であった．
を用い，制御アルゴリズム 1 を 5 回反復適用した結果で
ある．w の初期値は 0, φ の初期値は
¡
¢T
φ∗T = 1.74, 0.63, 0, 0, −1.74, −0.63
(14)
である．また Fig.6 には，形状 φ の応答を毎回 Step 2 終
了時のみ取り出したもの (Poincaré 断面) をプロットした
ものである．このときは上と同条件で反復 25 回まで行っ
ている．同様の安定サイクルは回転制御の場合にも存在す
ることが確認された．
0.4
y
0.2
0
-0.2
3.2
周期入力下での不変サイクルの条件
θ0
-0.4
-0.6
本稿では，制御アルゴリズム 1 の実行前後で形状が保
たれる条件という意味で，式 (11) および式 (11) を不変条
件と呼ぶことにする．
まず，並進制御の場合にどのような φ が不変条件を満
たすかを考えよう．対称性を考慮して，φ21 = φ22 = 0
かつ φ11 = −φ31 （これを簡単のため φ1 と表記する），
φ12 = −φ32（同じく φ2 と表記）に固定する．すると不変
条件は，g1 と g23 の第 1 行・第 2 行の平行条件から，
-0.8
x
-1
-1.2
0
5
に帰着される．Fig.3 に ιT (φ1 , φ2 ) の概形とその零化解の
曲線 (ιT = 0) を示す．このような不変条件を満たす形状 φ
の集合を ΦT とおくと，ΦT は 1 次元多様体の形に分布し
ていることがわかる．(10) 式で用いていた φT もまた ΦT
に属する形状の一つであるにすぎない．
15
20
u1
0
5
10
15
20
1
0.5
0
-0.5
-1
u2
0
2
2
ιT (φ1 , φ2 ) := sin(φ1 + π) + sin(φ1 + φ2 + π) = 0 (12)
3
3
10
1
0.5
0
-0.5
-1
5
10
15
20
1
0.5
0
-0.5
-1
u3
0
5
10
15
20
Figure 4: Simulation result (2 links, translation)
Initial Position
2
1.5
1
0.5
0
-0.5
-1
-1.5
-2
3
2
1
-3
-2
0
-1
0
-1
1
φ1
第１関節
-2
2
3 -3
φ2
第2関節
End of Step 1
Figure 3: Invariant Condition
なお，回転制御の場合には形状を回転対称 φ11 = φ21 =
φ31 (=: φ1 ), φ12 = φ22 = φ32 (=: φ2 ) に固定すると，不変
条件は
ιR (φ1 , φ2 ) := 2+cos φ1 +2 cos φ2 +cos(φ1 +φ2 ) = 0 (13)
Figure 5: Snapshots
2
φ12
1
0.5
φ21
φ22
0
-0.5
安定サイクルの存在と周期フィードバック制御
-1.5
さて，このような解析に基づき，さまざまな基準形状
φT ∈ ΦT を初期値として制御アルゴリズム 1 の数値シミュ
レーションを行ったところ，興味深い現象がみられた．あ
る特定の基準形状 φ∗T はこのアルゴリズムのもとで安定な
サイクルの上にあり，先行研究とは違って持続的な推進が
可能となったのである．
その様子を Fig. 4 に示す．上から順に w の応答，制御入
力である．Fig.5 には 1 周期分の運動のスナップショットを
示す．制御系のパラメータとしては A = 0.2, ω = 2[rad/s]
φ11
1.5
に帰着され，条件を満たす形状の集合 ΦR がやはり 1 次元
多様体の形をなすことがわかっている．
3.3
End of Step 2
φ32
φ31
-1
-2
0
20
40
60
80
100
Figure 6: Poincaré map
このように並進・回転ともに安定なサイクルが見出され
たことにより，Fig.7 のように各周期毎に所望の位置・姿
勢をフィードバックして適切な移動プリミティブを実行す
ることが可能になった．この例では初期形状として φ = 0
を与えているが，並進制御を反復するにつれて上図のよう
に φ∗T に近い形状に自動的に収束していき，さらに回転制
90
御を組み合わせることによって自由に旋回させることに成
功している．
80
kf k
ConstraintForce[N]
70
minimized
(proposed)
60
50
40
30
20
10
0
0
5
10
15
20
25
t[s]
30
35
40
45
90
80
kτ k
minimized
Figure 7: Simulation result (free motion)
ConstraintForce[N]
70
60
50
40
30
20
10
動力学モデルにおけるトルク入力設計法
4
0
0
5
10
15
20
25
30
35
40
45
t[s]
本節では，制御対象を動力学モデル (7)(8) とし，関節トル
ク τ を入力として運動制御を行う問題について論じる．手
順そのものはすでに知られた手法の直接適用であるので詳
述することは避け，結果を中心に述べる．
軌道追従制御
4.1
前節までに述べた運動学モデルに基づく方法によって
決定されるのは速度入力 u(t) = ẇ(t) であるが，本稿では
ここから参照軌道を生成してこれに追従させる方法をと
る．まず ẇr (t) := u(t) を参照速度とし，その積分 wr (t)
を参照軌道とする．w(t) を追従 wr (t) に追従させるため
に，次のような規範モデルを考える．
Mm (ẅ − ẅr ) + Dm (ẇ − ẇr ) + Km (w − wr ) = 0 (15)
Mm , Dm , Km は設計パラメータである．(15) 式をを実現
するためには，逆動力学条件
Figure 9: Constraint Force kf k
最小化することは容易である (具体的なアルゴリズムは同
文献を参照)．
以下に，2 リンクモデルの推進制御の軌道に本節の方法
を用いて追従させた結果を示す．Fig.8 は w と wr の応答
であるが，過渡応答による追従遅れにもほとんど影響され
ずに推進制御が実現できていることが分かる．
Fig.9 の上図はこのときの拘束力のノルムである．比較
のために，通常よく行われるように左擬似逆行列 (ΩT )+
を用いて求めたトルクによる結果を下図に示しておく．こ
の場合は (16) 式を満たす τ の中で kτ k が最小のものを選
んでいることになる．後者と比べると拘束力が 50%程度
に低減され，実現性が向上していることがわかる．
5
おわりに
本研究では，三叉ヘビ型ロボットに対する先行研究の制御
−1
ΩT τ = M̂ (−Mm
(Dm (ẇ−ẇr )+Km (w−wr ))−ẅr +D̂ẇ+Ĉ) 方法を改良し，各足 2 リンクのモデルに周期入力制御を
(16)
行った場合に不変なサイクルが分布していること，またそ
を満たす関節トルク τ を与えればよい．
の中の安定なサイクルを利用して持続的かつロバストな
運動制御を実現した．また対象を動力学モデルに拡張し，
4.2 拘束力の最小化
関節トルク入力による軌道追従制御を実現するとともに冗
長性を利用した車輪拘束力の低減化を行った．
Ω が列フルランクであれば (16) 式を満たす τ は存在す
なお本研究は，文部科学省科学研究費補助金（若手研究
T
るが，ker Ω に対応する (3` − 3) 次元の自由度があるの
(B) No.14750361）による支援を受けて行われた．ここに
で一意ではない．そこで，本稿では文献 [7] の方法を採用
感謝の意を表する．
し，この自由度を利用して床面から車輪にかかる拘束力 f
がなるべく小さくなるように τ を決定する．f は τ につ
References
いてアフィンな関数になるので，(16) 式のもとで kf k を
[1] P. Prautsch and T. Mita. Control and analysis of the gait of snake robots.
In International Conference of Control Applications, pp. 502–507, 1999.
0.15
Z
[
ǰ
ZT
[T
ǰ T
0.1
0.05
[2] 石川. 非ホロノミックシステムの位相構造–未解決問題への誘い–. シ
ステム/制御/情報, Vol. 45, No. 9, pp. 536–543, 2001.
[3] 石川. 複雑な可制御性を持つ非ホロノミックシステムの制御. 計測と
制御, Vol. 42, No. 10, pp. 841–846, 2003.
[4] 岩谷, 石川, 原. 三叉ヘビ型移動ロボットの解析と運動制御. 計測自動
制御学会論文集, Vol. 39, No. 12, pp. 1159–1161, 2003.
0
[5] 岩谷, 石川, 原. 一次の可制御構造を持つ対称アフィン系のフィード
バック制御–時間軸状態制御系に基づく切り替え制御戦略–. 計測自
動制御学会論文集, Vol. 38, No. 10, pp. 839–844, 2002.
-0.05
-0.1
-0.15
0
5
10
15
20
25
30
35
40
45
t[s]
Figure 8: Trajectory Tracking Control
[6] 石川. 多関節三叉ヘビ型移動ロボットの解析と制御. 第３回 SICE 制
御部門大会, pp. 601–606, 2003.
[7] 伊達, 三平, 中浦. 拘束力を考慮した蛇型ロボットの推進制御. 第 1 回
SICE 制御部門大会, pp. 215–220, 2001.