受動性に基づくモバイルマニピュレータの制御
電子機械工学専攻 2 年 5 番
指導教員
1
はじめに
近年, 地震などの自然災害が増え, 災害現場では
人間が立ち入ることすらできない場所や危険な場
所が多く存在する. そこで, 危険な場所で人間が作
業を行う代わりに, ロボットが作業を行うことに注
目が集まっており, いくつかの作業ロボットが実際
の作業現場で活躍している例もある. これにより
人間の危険回避はもちろん, 作業効率の向上も期
待され, 早急な作業ロボットの導入が求められてい
る. その作業ロボットの中の 1 つにモバイルマニ
ピュレータというロボットがあげられる. これは
台車型ロボットに多関節マニピュレータを搭載し
たロボット (図 1) であり, 災害現場をはじめ, 福祉
施設や工場内での活躍が期待され, 今後の重要な
役割を担っている.
従来研究では, 文献 [1] において, モバイルマニ
ピュレータの位置と力のハイブリッド制御が行わ
れている. ここでは, 適応制御を用いた制御則 [1, 2]
が提案されており, 目標位置と姿勢が目標値に収
束することをシミュレーションにより示している.
本研究では, 受動性に基づいたモバイルマニピュ
レータの制御を考える. モバイルマニピュレータ
は, 台車型ロボットに 2 自由度マニピュレータを載
せたものである. そこで, 台車型ロボットの運動学
モデルとダイナミクス, 2 自由度マニピュレータの
ダイナミクスからモバイルマニピュレータのモデ
ルを導出する. そして, 導出したモデルのエネル
ギー関数を用いてリアプノフの安定定理から制御
則を提案することを目指す. さらに, MATLAB で
のシミュレーションにより提案する制御則の検証
を行う. また, 外力が存在する場合において, 提案
した制御則より, 外力がシステムに与える影響を
小さくできるかを確認する.
大浦佑太
河合康典
車輪の回転角度を θBr , θBl で表すと, ベースの一般
化座標 qB は,
qB = xB yB θB θBr θBl
T
となる. また, 図 3 のような 2 自由度マニピュレー
タの各リンクが, 鉛直軸と直行する軸周りの回転
qM 1 , qM 2 を行うとする. このとき, マニピュレー
タの一般化座標 qM は,
qM = qM 1 qM 2
T
である. これより, モバイルマニピュレータの一般
化座標 q を以下のように定義する.
T
T
q = qB
qM
T
モバイルマニピュレータの駆動トルクは, ベース
の車輪を駆動する τBr , τBl, マニピュレータの各軸
に働く τM 1 , τM 2 である. ベースの幅 2bB , 車輪の
半径 rB , 車輪軸中心とマニピュレータ間の距離 d,
マニピュレータのアームの長さ l1 , l2, マニピュレー
タ軸からリンク質量中心までの長さ r1 , r2 , ベース
と各車輪の質量 mB , mW , マニピュレータのアーム
の質量 m1 , m2, ベースの慣性モーメント IB , 車輪
の軸についての車輪の慣性モーメント IW , 車輪の
回転についての慣性モーメント Im , マニピュレー
タのアームの慣性モーメント IM 1 , IM 2 とする.
ベースの運動学モデルは,
cos θB
0
xB
yB sin θB
0
uB
d
θB = 0
1
dt
1
bB ωB
θBr rB
rB
1
θBl
− rbBB
rB
(1)
で表される. uB はベースの進行方向速度, ωB は回
転角速度である. また, uB , ωB は, ベースの両車
輪の角速度 νB = [θ˙Br θ˙Bl ]T と次のような関係が
ある.
図. 1: モバイルマニピュレータ
2
モバイルマニピュレータのモデル化
図 2 のように台車型ロボット (以下, ベースと呼
ぶ) の車輪軸の中心座標を (xB , yB , θB ), 左右の
uB
=
ωB
rB
2
rB
2bB
rB
2
rB
− 2b
B
θ˙Br
θ˙Bl
(2)
したがって, ベースの運動学モデル (2) 式は, (3) 式
y
したモバイルマニピュレータのダイナミクス (7) 式
が得られる.
2rB
θB
yB
T
M1 (q)ν˙ + C1 (q, q)ν
˙ + G1 (q) = B1 (q)τ + J¯M
(q)λM
(7)
2bB
0
xB
z
リンク2
r2
r1
l2
リンク1
l1
m1 qM 1
m2
qM 2
:質量中心
M1 (q)ν˙ + C1 (q, q)ν
˙ + G1 (q) = B1 (q)τ
xM
x
図. 3: 2 自由度マニピュレータ
で表すこともできる.
rB
cos θB
xB
r2B
yB 2 sin θB
d
rB
θB =
2bB
dt
θ
1
Br
θBl
0
rB
2
rB
2
= SB (qB )νB
νd = θ˙Brd θ˙Bld q˙M 1d q˙M 2d
cos θB
sin θB ˙
θBr
rB
− 2b
θ˙
B
Bl
0
1
ξ := ν − νd
(3)
SB (qB ) 0
νB
, ν=
0
I2
q˙M
(9)
と定義する. ここで, 入力トルクとして,
B1 (q)τ = M1 (q)ν˙ d + C1 (q, q)ν
˙ d + G1 (q) + uξ
(10)
を考える. uξ は後に提案する新たな入力である.
モバイルマニピュレータのダイナミクス (8) 式と
入力トルク (10) 式を用いて構成されるシステムは,
(4)
ただし,
S(qB ) =
T
とする. そして, 車輪角速度および関節角速度に関
する偏差 ξ ∈ R4 を
そして, (3) 式のベースの運動学モデルを用いて,
モバイルマニピュレータの運動学モデルを以下の
ように表すことにする.
q˙ = S(qB )ν
(8)
ベースの両車輪の目標角速度 νBd = [θ˙Brd θ˙Bld ]T
と, マニピュレータの目標関節角速度 q˙M d =
[q˙M 1d q˙M 2d ]T を合わせることで, モバイルマニピュ
レータの目標値を
接続部2
接続部1
システムの構築
本研究では, モバイルマニピュレータのダイナミ
クス (7) 式を簡単化するために外力がない, つまり
λM = 0 とする. このとき, モバイルマニピュレー
タのダイナミクスは次式となる.
図. 2: 台車型ロボット
zM
0
3
x
M1 (q)ξ˙ = −C1 (q, q)ξ
˙ + uξ
(5)
(11)
である. このシステムに対して次の補題が成立する.
また, モバイルマニピュレータのダイナミクスは
次式で与えられる.
補題 1 (11) 式において, 入力 uξ から出力 ξ に対
して
T
M (q) q¨ + C (q, q)
˙ q˙ + G (q) = JM
(q) λM + B(q)τ
T
(6)
0
M (q) ∈ R7×7 は正定な慣性行列, C(q, q)
˙ ∈
R7×7 は遠心力を表す行列, G(q) ∈ R7 は重力項,
JM (q) ∈ R2×7 はヤコビアン行列, B(q)τ ∈ R7 は
入力（駆動トルク）, λM ∈ R2 は外力である.
(6) 式のモバイルマニピュレータのダイナミクス
の変数 q に運動学モデル (3) 式を代入し, 両辺の左
から S T (qB ) を掛けることで, 運動学モデルを考慮
uTξ ξdt ≥ −βξ , ∀ T > 0
(12)
が成立する. ただし, βξ はダイナミクスの初期条
件のみに関係する非負の定数である.
証明 1 エネルギー関数として
Vξ =
2
1 T
ξ M1 (q)ξ
2
(13)
を考える. エネルギー関数 Vξ を時間微分する.
5
1 ˙T
V˙ ξ =
ξ M1 (q)ξ + ξ T M˙ 1 (q)ξ + ξ T M1 (q)ξ˙
2
1
= ξ T M1 (q)ξ˙ + ξ T M˙ 1 (q)ξ (... Vξ ∈ R)
2
(14)
外力が存在する場合, 外力 λM が被制御量 ξ に
与える影響を小さくすることが求められる. この
ために, システムの L2 ゲインを小さくするコント
ローラを設計する必要がある.
外力 λM が加わったときのシステムは, (7) 式と
(10) 式より,
ここで, (11) 式を代入すると,
1
V˙ ξ = ξ T −C1 (q, q)ξ
˙ + uξ + ξ T M˙ 1 (q)ξ
2
1
˙ ξ (15)
= ξ T uξ + ξ T M˙ 1 (q) − 2C1 (q, q)
2
(15) 式の第 2 項目の M˙ 1 (q) − 2C1 (q, q)
˙ は歪対称行
T
M1 (q)ξ˙ = −C1 (q, q)ξ
˙ + uξ + J¯M
(q)λM (22)
となる. このシステム (22) 式を, (14) 式に代入す
ると,
T
V˙ ξ = ξ T −C1 (q, q)ξ
˙ + uξ + J¯M
(q)λM
列であるため,
ξ T M˙ 1 (q) − 2C1 (q, q)
˙ ξ=0
1
+ ξ T M˙ 1 (q)ξ
2
= ξ T uξ + ξ T J¯T (q)λM
(16)
となる. したがって,
M
V˙ ξ = ξ T uξ = uTξ ξ
(17)
=
(17) 式の両辺を時間積分することで,
T
0
uTξ ξdt =
T
この補題 1 は, モバイルマニピュレータのダイ
ナミクス (8) 式と入力トルク (10) 式を用いて構成
されるシステムが, 受動性を持っていることを意味
する.
制御則の提案
Kξ := diag[kξ1, kξ2 , kξ3, kξ4 ]
(20)
定理 1 システム (11) 式と入力 (19) 式で構成され
るシステム全体の平衡点 ξ = 0 は漸近安定である.
となる. このとき最悪外乱として, λM は,
証明 2 (13) 式のエネルギー関数は, 行列 M1 (q) が
正定であるため正定関数である. さらに, (19) 式を
適用したときのエネルギー関数 Vξ の時間微分は,
V˙ ξ =
T
(23)
1
γ2
V˙ ξ + ξ 2 −
λM 2
2
2
γ2
1
T
=−
λM 2 + ξ 2 + ξ T uξ + ξ T J¯M
(q)λM
2
2
1
1
=
ξ 2 − ξ T Kξ ξ + 2 ξ T Jξ
2
2γ
2
γ
1
−
λM − 2 J¯M (q)ξ 2 (... (19), (24) 式)
2
γ
(25)
Kξ は各車輪および各関節に対するゲインである.
ただし, ゲインにおける各要素は全て正とする.
このとき, システム (11) 式の受動性によって, 安
定性に関しての次の定理が導かれる.
uTξ ξ
2
T (q)J¯ (q) とした. さらに,
となる. ただし, J = J¯M
M
システム (11) 式に対して, 平衡点 ξ = 0 を安定
とする制御則として次式を提案する.
(19)
γ2
γ2
λM 2 −
λM
2
2
T
+ξ T uξ + ξ T J¯M
(q)λM
γ2
1
λM − 2 J¯M (q)ξ 2
2
γ
T
γ2
1
1
=
λM − 2 J¯M (q)ξ
λM − 2 J¯M (q)ξ
2
γ
γ
2
γ
1
T
=
λM 2 − ξ T J¯M
(q)λM + 2 ξ T Jξ
2
2γ
(... Vξ ∈ R) (24)
(Q.E.D.)
uξ = −Kξ ξ
(... (16) 式)
1
1
2
+ ξ 2− ξ
2
2
ここで, 平方完成を用いるために, 次式を導出する.
V˙ ξ dt ≥ −Vξ (0) := −βξ (18)
0
が成立する.
4
外力が存在する場合
λM =
1 ¯
JM (q)ξ
γ2
(26)
で与えられる. 任意の λM に対して, 以下の不等式
が成り立つ.
T
= −(Kξ ξ) ξ = −ξ Kξ ξ ≤ 0 (21)
1
γ2
V˙ ξ + ξ 2 −
λM 2
2
2
1
1
≤
ξ 2 − ξ T Kξ ξ + 2 ξ T Jξ
2
2γ
1
1
= −ξ T Kξ − 2 J − I ξ
2γ
2
となり, 準負定となる. つまり, リアプノフの安定
定理より平衡点 ξ = 0 は安定となる. また, ラ・
サールの不変性原理を用いると, ξ = 0 以外では
V˙ ξ = 0 にはならないため, 平衡点 ξ = 0 は漸近安
定である.
(Q.E.D.)
3
(27)
(28)
2
ここで, 行列 P を
−
γ2
λM
2
−8
−5
0
5
10
15
20
1.6
(γ λM
0.4
0.2
0
−0.2
0
10
20
50
0
−0.2
0
10
20
t[s]
V˙ ξ dt ≥ −2V (0) := −βL2
1.8
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2
0
−0.2
0
γ≥
sup
λM ∈L2 /{0}
ξ L2
λM L2
30
40
50
t[s]
図. 6: 右車輪の角速度 θ˙Br
(31)
となる. βL2 はダイナミクスの初期条件のみに関
係する非負の定数である. そして, T → ∞ のとき
に, 初期値を無視すれば,
図. 7: 左車輪の角速度 θ˙Bl
1
0.8
0.6
0.4
0.2
10
(32)
20
30
t[s]
40
50
0
−0.2
0
10
20
30
40
50
t[s]
図. 8: リンク 1 の関節角 図. 9: リンク 2 の関節角
度 qM 1
度 qM 2
となる. したがって, 行列 P が準正定行列のとき,
システムは外力 λM から被制御量 ξ に対して, γ 以
下の L2 ゲインを有する. つまり, γ が 1 以下のコン
トローラを設計すれば, L2 ゲインが小さくなり, 外
力 λM が被制御量 ξ に与える影響を小さくできる.
6
40
qM 2 , qM 2d[rad]
0
30
qM 1 , qM 1d[rad]
T
1
0.6
0
≥2
図. 5: ベースの傾き θB
0.8
0.6
− ξ )dt
50
1.2
1
0.8
2
40
1.4
1.2
0.2
2
30
1.6
1.4
0.4
2
20
t[s]
図. 4: モバイルマニピュ
レータの軌道
が成り立つ. (30) 式の両辺を時間積分すると,
T
10
X[m]
≤ −ξ T P ξ ≤ 0 (30)
2
−4
θ˙Br , θ˙Brd [rad/s]
2
−2
−6
と定義し, この行列 P が準正定行列となるように,
ゲイン Kξ を選ぶと,
1
V˙ ξ + ξ
2
θB [rad]
(29)
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1.2
−1.4
−1.6
−1.8
−2
0
θ˙Bl , θ˙Bld [rad/s]
1
1
P := Kξ − 2 J − I
2γ
2
Y[m]
0
7
おわりに
本研究では, ベースとマニピュレータのダイナ
ミクスからモバイルマニピュレータのダイナミク
スを求めた. そして, そのダイナミクスに運動学モ
デルを導入することで, 運動学モデルを考慮した
ダイナミクスを導出した. そして, 導出したモバイ
ルマニピュレータのモデルのエネルギー関数を用
いて, リアプノフの安定定理から制御則を提案し,
シミュレーションにより提案する制御則の検証を
行った. その結果, 設定した目標軌道にモバイル
マニピュレータが追従していることが確認できた.
また, 外力が存在する場合については, 提案した制
御則より, システムが γ 以下の L2 ゲインを有し,
外力の影響を小さくできることを確認した.
シミュレーション
モバイルマニピュレータの物理パラメータを
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 とする.
ベースの目標軌道は, 直線と曲線から成る混成軌
道とする. また, マニピュレータの各リンクの目標
軌道は, 始動時と停止時に等加速度運動をする軌道
とする. 以上の目標軌道にモバイルマニピュレー
タが追従するかを MATLAB でシミュレーション
する. ただし, 本シミュレーションでは外力 λM は
考慮しない.
参考文献
[1] 成清, 中川, 川西, “モバイルマニピュレータ
の位置と力のハイブリッド制御, ” 計測自動
制御学会論文集, vol. 44, no. 11, pp. 919-926,
2008.
シミュレーションの結果を図 4-9 に示す. 各図に
おいて, 鎖線が目標軌道, 実線がモバイルマニピュ
レータの状態である. 図 4-9 の全ての図で, 設定し
た目標軌道とモバイルマニピュレータの状態が重
なっていることが確認できる.
[2] 深尾, “非ホロノミック移動ロボットの適応制
御, ” 計測と制御, vol. 45, no. 7, pp. 602–607,
2006.
4
© Copyright 2024 ExpyDoc
