専攻分野の名称
工学
専攻の区分
電気電子工学
テーマ名: 受動性に基づくモバイル
マニピュレータの制御
キーワード科目: 自動制御
氏名: 大浦 佑太
目次
目次
第1章
序論
1
1.1 研究の背景と動機 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
1.2 研究の目的 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
第2章
モバイルマニピュレータのモデル化
3
2.1 モデル化の準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
2.2 モバイルマニピュレータの運動学モデル . . . . . . . . . . . . . . . . . . . .
4
2.3 モバイルマニピュレータのダイナミクス . . . . . . . . . . . . . . . . . . . .
5
2.4 運動学モデルを考慮したダイナミクス
7
第3章
. . . . . . . . . . . . . . . . . . . . .
システムの構築
9
3.1 ダイナミクスの歪対称性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 入力トルクの導入とシステムの受動性
9
. . . . . . . . . . . . . . . . . . . . . 10
3.3 制御則 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
第4章
シミュレーション
13
4.1 MATLAB でのシミュレーション . . . . . . . . . . . . . . . . . . . . . . . . 13
4.2 シミュレーション結果 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
第5章
結論
16
5.1 研究の成果 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
参考文献
17
1
第 1章
序論
1.1
研究の背景と動機
近年, 地震などの自然災害が増え, 災害現場では人間が立ち入ることすらできない場所
や危険な場所が多く存在する. そこで, 危険な場所で人間が作業を行う代わりに, ロボッ
トが作業を行うことに注目が集まっており, いくつかの作業ロボットが実際の作業現場
で活躍している例もある. これにより人間の危険回避はもちろん, 作業効率の向上も期待
され, 早急な作業ロボットの導入が求められている. その作業ロボットの中の 1 つにモバ
イルマニピュレータというロボットがあげられる. これは台車型ロボットに多関節マニ
ピュレータを搭載したロボット (図 1.1) であり, 災害現場をはじめ, 福祉施設や工場内で
の活躍が期待され, 今後の重要な役割を担っている.
従来研究では, 文献 [1] において, モバイルマニピュレータの位置と力のハイブリッド
制御が行われている. ここでは, 適応制御を用いた制御則 [1, 2] が提案されており, 目標
位置と姿勢が目標値に収束することをシミュレーションにより示している.
図 1.1: モバイルマニピュレータ
第 1 章 序論
2
1.2
研究の目的
本研究では, 受動性に基づいたモバイルマニピュレータの制御を考える. モバイルマニ
ピュレータは, 台車型ロボットに 2 自由度マニピュレータを載せたものである. そこで, 台
車型ロボットの運動学モデルとダイナミクス, 2 自由度マニピュレータのダイナミクスか
らモバイルマニピュレータのモデルを導出する. そして, 導出したモデルのエネルギー関
数を用いてリアプノフの安定定理から制御則を提案することを目指す. さらに, MATLAB
でのシミュレーションにより提案する制御則の検証を行う.
3
第 2章
モバイルマニピュレータのモデル化
2.1
モデル化の準備
図 2.1 のように台車型ロボット (以下, ベースと呼ぶ) の車輪軸の中心座標を (xB , yB , θB ),
左右の車輪の回転角度を θBr , θBl で表すと, ベースの一般化座標 qB は,
h
iT
qB = xB yB θB θBr θBl
となる. また, 図 2.2 のような 2 自由度マニピュレータの各リンクが, 鉛直軸と直行する
軸周りの回転 qM 1 , qM 2 を行うとする. このとき, マニピュレータの一般化座標 qM は,
h
iT
qM = qM 1 qM 2
である. これより, モバイルマニピュレータの一般化座標 q を以下のように定義する.
q=
h
T
qB
T
qM
iT
h
iT
= xB yB θB θBr θBl qM 1 qM 2
モバイルマニピュレータの駆動トルクは, ベースの車輪を駆動する τBr , τBl , マニピュ
レータの各軸に働く τM 1 , τM 2 である. ベースの幅 2bB , 車輪の半径 rB , 車輪軸中心とマニ
ピュレータ間の距離 d, マニピュレータのアームの長さ l1 , l2 , マニピュレータ軸からリン
ク質量中心までの長さ r1 , r2 , ベースと各車輪の質量 mB , mW , マニピュレータのアーム
の質量 m1 , m2 , ベースの慣性モーメント IB , 車輪の軸についての車輪の慣性モーメント
IW , 車輪の回転についての慣性モーメント Im , マニピュレータのアームの慣性モーメン
ト IM 1 , IM 2 とする.
第 2 章 モバイルマニピュレータのモデル化
4
y
z
2rB
qB
yB
リンク2
zM
r2
r1
2bB
0
xB
x
図 2.1: 台車型ロボット
2.2
m
qM 22
リンク1
l1
m1
l2
qM 1
:質量中心
接続部2
xM
0
x
図 2.2: 2 自由度マニピュレータ
モバイルマニピュレータの運動学モデル
ベースの運動学モデルは,
x
cos θB
0
B
yB sin θB
0
uB
d
θ = 0
1
B
dt
ωB
1
bB
θBr rB
r
B
1
bB
θBl
−
rB
rB
(2.1)
で表される. uB はベースの進行方向速度, ωB は回転角速度である. また, uB , ωB は, ベー
スの両車輪の角速度 νB = [θ̇Br θ̇Bl ]T と次のような関係がある.
rB
rB
u
2 θ̇Br
B = 2
rB
rB
ωB
θ̇Bl
2bB − 2bB
したがって, ベースの運動学モデル (2.1) 式は (2.4) 式で表すこともできる.
rB
rB
x
cos θB 2 cos θB
B 2
r
r
yB B sin θB B sin θB
2
2
θ̇Br
d
θ = rB
rB
−
B
2bB
2bB
dt
θ̇Bl
1
0
θBr
θBl
0
1
= SB (qB )νB
(2.2)
(2.3)
(2.4)
続いて, (2.4) 式のベースの運動学モデルを用いて, モバイルマニピュレータの運動学モデ
ルを求める. この運動学モデルを以下のように表すことにする.
q̇ = S(qB )ν
(2.5)
2.3. モバイルマニピュレータのダイナミクス
5
ただし,
SB (qB ) 0
ν
, ν = B
S(qB ) =
0
I2
q̇M
(2.6)
ν は, ベースの両車輪の角速度 νB とマニピュレータの一般化座標の微分 q̇M を並べたも
のなので, モバイルマニピュレータの物理量を示している. (2.6) 式の S(qB ) はモバイル
マニピュレータの物理量 ν と一般化座標 q の関係を表す行列である. この行列では, 物理
量 ν の q̇M と一般化座標の微分 q̇ の q̇M が同じものであるため, 単位行列 I2 を使用してい
る. そして, SB (qB ) はベースの両車輪の角速度 νB と, 単位行列 I2 はマニピュレータの一
般化座標の微分 q̇M とそれぞれが独立して関係を持っているため, (2.6) 式の S(qB ) のよう
な配置の行列が作られる.
2.3
モバイルマニピュレータのダイナミクス
図 2.2 より, 2 自由度マニピュレータの手先の座標 (xM , zM ) は以下で表される.
xM = l1 cos qM 1 + l2 cos(qM 1 + qM 2 )
zM = l1 sin qM 1 + l2 sin(qM 1 + qM 2 )
(2.7)
ここで, マニピュレータのヤコビアン JM (q) を求める. JM (q) は
h
i
JM (q) = JM 0 JM 2
とする. JM 0 , JM 2 には, それぞれ次の関係がある.
∂ xM
= JM 0
∂qB y
(2.8)
(2.9)
M
∂ xM
= JM 2
∂qM y
M
(2.10)
したがって, JM (q) は以下で与えられる.
0 0 0 0 0 −l1 sin qM 1 − l2 sin(qM 1 + qM 2 ) −l2 sin(qM 1 + qM 2 )
(2.11)
JM (q) =
0 0 0 0 0
l1 cos qM 1 + l2 cos(qM 1 + q2 )
l2 cos(qM 1 + qM 2 )
第 2 章 モバイルマニピュレータのモデル化
6
このとき, モバイルマニピュレータのダイナミクスは次式で与えられる.
T
M (q) q̈ + C (q, q̇) q̇ + G (q) = JM
(q) λM + B(q)τ
(2.12)
M(q) ∈ R7×7 は正定な慣性行列, C(q, q̇) ∈ R7×7 は遠心力を表す行列, G(q) ∈ R7 は重力
項, JM (q) ∈ R2×7 はヤコビアン行列（拘束に関する行列）, B(q) ∈ R7×4 , τ ∈ R4 は入力
（駆動トルク）, λM ∈ R2 は外力（拘束力）である. また, 各行列の成分は以下に示す. た
だし, 表記を簡略化するために次のように置く.
SB := sin θB , CB := cos θB , S1 := sin qM 1 , C1 := cos qM 1
S2 := sin qM 2 , C2 := cos qM 2 , S12 := sin(qM 1 + qM 2 ), C12 := cos(qM 1 + qM 2 )
m = mB + 2mw , I = mB d2 + 2mw b2 + IB + 2Im
M1 = m1 r12 + m2 l12 + IM 1 , M2 = m2 r22 + IM 2 , R = m2 l1 r2
MB
0
M(q) =
0 MM
m
0
−mB dSB 0
0
0
0
0
m
m
dC
0
0
0
0
B
B
I
0
0
0
0
−mB dSB mB dCB
=
0
0
0
IW 0
0
0
0
0
0
0
I
0
0
W
0
0
0
0
0 M1 + M2 + 2RC2 M2 + RC2
0
0
0
0
0
M2 + RC2
M2
CB
C(q, q̇) =
0
0
0
0
0
=
0
CM
0
0
0
0 −mB dθ˙B CB 0 0
0
0 −mB dθ˙B SB 0 0
0
0
0
0 0
0
0
0
0 0
0
0
0
0 0
0
0
0
0 0 −Rq̇M 2 S2
0
0
0 0
Rq̇M 1 S2
0
0
0
0
0
−R(q̇M 1 + q̇M 2 )S2
0
2.4. 運動学モデルを考慮したダイナミクス
7
0
0
0
0
=
G(q) =
0
GM
0
m gr C + m g(l C + r C )
2
1 1
2 12
1 1 1
m2 gr2 C12
0 0 0 0 0 −l1S1 − l2 S12 −l2 S12
JM (q) =
0 0 0 0 0 l1C1 + l2C12
l2 C12
0 0 0 0
0 0 0 0
τBr
0 0 0 0
τ
λM 1
τB
BB 0
Bl
, τ = =
= 1 0 0 0 , λM =
B(q) =
τ
λ
τ
0 I2
M 1
M2
M
0 1 0 0
τM 2
0 0 1 0
0 0 0 1
2.4
運動学モデルを考慮したダイナミクス
(2.12) 式のモバイルマニピュレータのダイナミクスの変数 q に運動学モデル (2.5) 式を
代入することで, 運動学モデルを考慮したモバイルマニピュレータのダイナミクス (2.13)
式が得られる.
T
M1 (q)ν̇ + C1 (q, q̇)ν + G1 (q) = B1 (q)τ + J M (q)λM
(2.13)
第 2 章 モバイルマニピュレータのモデル化
8
M1 (q) = S T (qB )M(q)S(qB )
2
rB
2
4b2B (mbB + I) + IW
2
rB
2
4b2B (mbB − I)
=
0
0
2
rB
(mb2B
4b2B
2
rB
(mb2B
4b2B
− I)
+ I) + IW
0
0
0
M1 + M2 + 2RC2
0
M2 + RC2
C1 (q, q̇) = S T (qB ){M(q)Ṡ(qB ) + C(q, q̇)S(qB )}
2
rB
0
0
0
2bB θ̇B mB d
r2
− B θ̇B mB d
0
0
0
2bB
=
0
0
−Rq̇M 22 S2 −R(q̇M 21 + q̇M 22 )S2
0
0
Rq̇M 21 S2
0
0
0
T
G1 (q) = S (qB )G(q) =
m gr cos q
+
m
g(l
C
+
r
C
)
1 1
2
1
1
2
12
M 21
m2gr2 C12
1 0 0 0
0 1 0 0
B1 (q) = S T (qB )B(q) =
0 0 1 0
0 0 0 1
0
0
0
0
T
T
J M = S T (qB )JM
(q) =
−l S − l S
1 1
2 12 l1 C1 + l2 C12
−l2 S12
l2 C12
0
0
M2 + RC2
M2
9
第 3章
システムの構築
3.1
ダイナミクスの歪対称性
前章で示した運動学モデルを考慮したモバイルマニピュレータのダイナミクス (2.13)
式を簡単化するために外力がない, つまり λM = 0 とする. このとき, モバイルマニピュ
レータのダイナミクスは次式となる.
M1 (q)ν̇ + C1 (q, q̇)ν + G1 (q) = B1 (q)τ
(3.1)
このダイナミクス (3.1) 式に関する重要な性質として以下の補題が成立する.
補題 3.1 Ṁ1 (q) − 2C1 (q, q̇) は歪対称行列となる.
証明
Ṁ1 (q) − 2C1 (q, q̇)
2
rB
0
− bB θ̇B mB d
0
0
r2
B θ̇B mB d
0
0
0
bB
=
0
0
0
R(2q̇M 1 + q̇M 2 )S2
0
0
−R(2q̇M 1 + q̇M 2 )S2
0
(3.2)
第 3 章 システムの構築
10
ここで, (3.2) 式を行列 A と置き, 歪対称行列の条件である A = −AT を用いる.
2
rB
0
− bB θ̇B mB d
0
0
r2
B θ̇B mB d
0
0
0
bB
T
−A =
0
0
0
R(2q̇M 1 + q̇M 2 )S2
0
0
−R(2q̇M 1 + q̇M 2 )S2
0
となり, 行列 A は歪対称行列である.
3.2
（Q.E.D.）
入力トルクの導入とシステムの受動性
ベースの目標速度を ηd = [uBd ωBd ]T とすると, (2.2) 式より, この目標速度 ηd に対応し
た両車輪の目標角速度 νBd = [θ̇Brd θ̇Bld ]T が求まる. そこで, ベースの目標車輪角速度を
νBd とし, マニピュレータの目標関節角速度 q̇M = [q̇M 1d q̇M 2d ]T と合わせることで, モバ
イルマニピュレータの目標値を
θ̇Brd
θ̇
Bld
νd =
q̇
M 1d
q̇M 2d
とする. そして, 車輪角速度および関節角速度に関する偏差 ξ ∈ R4 を
ξ := ν − νd
(3.3)
B1 (q)τ = M1 (q)ν̇d + C1 (q, q̇)νd + G1 (q) + uξ
(3.4)
と定義する.
ここで, 入力トルクとして
を考える. uξ は後に提案する新たな入力である.
モバイルマニピュレータのダイナミクス (3.1) 式と入力トルク (3.4) 式を用いて構成さ
れるシステムは以下のようになる.
M1 (q)ν̇ + C1 (q, q̇)ν + G1 (q) = M1 (q)ν̇d + C1 (q, q̇)νd + G1 (q) + uξ
M1 (q)(ν̇ − ν̇d ) = −C1 (q, q̇)(ν − νd ) + uξ
3.2. 入力トルクの導入とシステムの受動性
11
(3.3) 式より,
M1 (q)ξ̇ = −C1 (q, q̇)ξ + uξ
ξ̇ = −M1 (q)−1 C1 (q, q̇)ξ + M1 (q)−1uξ
(3.5)
このシステムに対して次の補題が成立する.
補題 3.2 (3.5) 式において, 入力 uξ から出力 ξ に対して
Z T
uTξ ξdt ≥ −βξ , ∀ T > 0
(3.6)
0
が成立する. ただし, βξ はダイナミクスの初期条件のみに関係する非負の定数である.
証明 エネルギー関数として
1
Vξ = ξ T M1 (q)ξ
2
(3.7)
を考える. エネルギー関数 Vξ を時間微分する.
1 T
V̇ξ =
ξ̇ M1 (q)ξ + ξ T Ṁ1 (q)ξ + ξ T M1 (q)ξ̇
2
Vξ ∈ R より
1
V̇ξ = ξ T M1 (q)ξ̇ + ξ T Ṁ1 (q)ξ
2
ここで, τd = 0 とした時の (3.5) 式を代入すると,
1
V̇ξ = ξ T (−C1 (q, q̇)ξ + uξ )ξ˙ + ξ T Ṁ1 (q)ξ
2
1
= ξ T uξ + ξ T Ṁ1 (q) − 2C1 (q, q̇) ξ
2
補題 3.1 の歪対称性を用いると,
ξ
T
Ṁ1 (q) − 2C1 (q, q̇) ξ = 0
となる. したがって,
V̇ξ = ξ T uξ = uTξ ξ
両辺を積分することで,
Z T
Z
T
uξ ξdt =
0
T
V̇ξ dt = Vξ (T ) − Vξ (0) ≥ −Vξ (0) := −βξ
(3.8)
0
が成立する.
（Q.E.D.）
この補題 3.2 は, モバイルマニピュレータのダイナミクス (3.1) 式と入力トルク (3.4) 式
を用いて構成されるシステムが受動性を持っていることを意味する.
第 3 章 システムの構築
12
3.3
制御則
システム (3.5) 式に対して, 平衡点 ξ = 0 を安定とする制御則として次式を提案する.
uξ = −Kξ ξ
0
0
kξ1 0
0 kξ2 0
0
Kξ :=
0
0 kξ3 0
0
0
0 kξ4
(3.9)
(3.10)
Kξ は各車輪および各関節に対するゲインである. ただし, ゲインにおける各要素は全て
正とする.
このとき, システム (3.5) 式の受動性によって, 安定性に関しての次の定理が導かれる.
定理 3.1 システム (3.5) 式と入力 (3.9) 式で構成されるシステム全体の平衡点 ξ = 0 は漸
近安定である.
証明 (3.7) 式のエネルギー関数をリアプノフ関数候補とする. このエネルギー関数は, 行
列 M1 (q) が正定であるため正定関数である. さらに, (3.9) 式を適用したときのエネルギー
関数 Vξ の時間微分は,
V̇ξ = uTξ ξ = −(Kξ ξ)T ξ = −ξ T Kξ ξ ≤ 0
(3.11)
となり, 準負定となる. つまり, リアプノフの安定定理より平衡点 ξ = 0 は安定となる. ま
た, ラ・サールの不変性原理を用いると, ξ = 0 以外では V̇ξ = 0 にはならないため, 平衡
点 ξ = 0 は漸近安定である.
13
第 4章
シミュレーション
MATLAB でのシミュレーション
4.1
モバイルマニピュレータの物理パラメータを bB = rB = d = 1[m], l1 = l2 = 2[m],
r1 = r2 = 1[m], mB = mW = m1 = m2 = 1[kg], IB = IW = Im = I1 = I2 = 1[kg · m2 ] のよ
うに与える. また, モバイルマニピュレータの初速度を q̇(0) = [0 0 0 0 0 0 0]T , 初期座標
を q(0) = [0 0 0 0 0 0 0]T とする.
ベースの目標速度 ηBd を (2.1) 式より適当に与え, 目標軌道 qBd = [xBd yBd θBd θBrd θBld ]T
を図 4.1 の混成軌道とする. また, マニピュレータの各リンクの目標関節角度 qM d =
[qM 1d qM 2d ]T を時間に応じて図 4.2 (実線：qM 1d , 鎖線：qM 2d ) のように与える. 以上の目
標値にモバイルマニピュレータが追従するかをシミュレーションする.
今回のシミュレーションを行う際に作成した simulink のモデルを図 4.3 に示す. シミュ
2
1.6
1.4
1.2
qM 1d, qM 2d[rad]
Y[m]
0
−2
−4
−6
−8
−5
0
5
10
15
X[m]
図 4.1: ベースの目標軌道
20
1
0.8
0.6
0.4
0.2
0
−0.2
0
10
20
30
40
50
t [s]
図 4.2: マニピュレータの目標関節角度
第 4 章 シミュレーション
14
dot(q)=S(qB)nu
Integrator1
q
1
s
nu
y0 s_qbu0
dot(q)
q
nu
xi
q
dot(q)
B(q)tau
B(q)tau
nu_d
dot(nu)_d
system
input torque
nu_d
0
Clock
Gain
t
dot(nu)_d
u_xi
K*u
desired value
図 4.3: システムのブロック図
レーションでは, 外力 λM を無視する. また, コントローラ Kξ は次のように設計する.
0
0
200 0
0 200 0
0
Kξ =
(4.1)
0
0 150 0
0
0
0 150
4.2
シミュレーション結果
シミュレーションの結果を図 4.4-4.9 に示す. 各図において, 鎖線が目標値, 実線がモバ
イルマニピュレータの状態である. 図 4.4 に, モバイルマニピュレータの軌道を示す. 図
4.5-4.7 に, ベースの傾き, 右車輪の角速度, 左車輪の角速度をそれぞれ示し, 図 4.8-4.9 に,
マニピュレータの各リンクの関節角度を示す.
図 4.4-4.9 の全ての図で, 設定した目標値とモバイルマニピュレータの状態が重なって
いることが確認できる. つまり, 提案した制御則で, モバイルマニピュレータが目標値に
追従していることがわかる.
4.2. シミュレーション結果
15
2
θB , θB d[rad]
Y[m]
0
−2
−4
−6
−8
−5
0
5
10
15
20
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1.2
−1.4
−1.6
−1.8
−2
0
1.6
1.4
1.4
1.2
1.2
θ̇Bl, θ̇Bld [rad/s]
θ̇Br , θ̇Brd [rad/s]
1.6
1
0.8
0.6
0.4
0.2
0
−0.2
0
10
20
30
40
10
20
30
40
50
t [s]
図 4.5: ベースの傾き θB
X[m]
図 4.4: モバイルマニピュレータの軌道
1
0.8
0.6
0.4
0.2
0
−0.2
0
50
t [s]
図 4.6: 右車輪の角速度 θ̇Br
10
20
30
40
50
t [s]
図 4.7: 左車輪の角速度 θ̇Bl
1.6
0.8
1.4
0.6
qM 2, qM 2d[rad]
qM 1, qM 1d[rad]
1.2
1
0.8
0.6
0.4
0.2
0.4
0.2
0
0
−0.2
0
10
20
30
40
50
t [s]
図 4.8: リンク 1 の関節角度 qM 1
−0.2
0
10
20
30
40
50
t [s]
図 4.9: リンク 2 の関節角度 qM 2
第 5 章 結論
16
第 5章
結論
5.1
研究の成果
本研究では, ベースの運動学モデルとダイナミクス, 2 自由度マニピュレータのダイナ
ミクスを考慮したモデルを考え, それに基づいた制御則をモバイルマニピュレータに適
用した. そこで, ベースのダイナミクスとマニピュレータのダイナミクスを組み合わせて
モバイルマニピュレータのダイナミクスを求めた. そして, そのダイナミクスに運動学モ
デルを導入することで, 運動学モデルを考慮したダイナミクスを導出した. そして, 導出
したモバイルマニピュレータのモデルのエネルギー関数を用いて, リアプノフの安定定
理から制御則を提案し, MATLAB でのシミュレーションにより提案する制御則の検証を
行った. その結果, 設定した目標値にモバイルマニピュレータが追従していることが確認
できた.
17
参考文献
[1] 成清, 中川, 川西, “モバイルマニピュレータの位置と力のハイブリッド制御, ” 計測
自動制御学会論文集, vol. 44, no. 11, pp. 919-926, 2008.
[2] 深尾, “非ホロノミック移動ロボットの適応制御, ” 計測と制御, vol. 45, no. 7, pp.
602–607, 2006.
© Copyright 2024 Paperzz
