2 次元視覚フィードバックシステムに対する非線形モデル予測制御○中 曽 裕 次 郎 · 村 尾 俊 幸 · 藤 田 政 之
東 京 工 業 大 学Nonlinear Model Predictive Control for Planar Visual Feedback System○ Yujiro Nakaso, Toshiyuki Murao and Masayuki Fujita
Tokyo Institute of Technology

This paper deals with a stabilizing receding horizon control for a planar dynamic visual feedback system with an
Eye-in-Hand conﬁguration. Firstly, the planar dynamic visual feedback system with the Eye-in-Hand conﬁguration
is given. Secondly, a CLF of the dynamic visual feedback system is deﬁned and obtained from a little algebraic
manipulation. Next, the stabilizing receding horizon control for the system is proposed. The use of the terminal costderived from a control Lyapunov function of the dynamic visual feedback system, which is obtained from the inverse
optimal control approach, is a key point. Finally, experimental results on the stabilizing receding horizon control areshown.

1

はじめに

CameraXw

Zw

化学プラントなどのプロセス制御で既に広く使われているモYw

デル予測制御は, リアルタイムで有限ホライズンの最適制御問題を解くことで得られる制御入力を用いた制御法である. 近年でTargetObjectは比較的高速なシステムに対するモデル予測制御が特に注目されている

1). このモデル予測制御の大きな問題点としては, リ

Manipulatorアルタイムで最適制御問題を解かなければならないことから, 計算時間が大きくなることが挙げられる. この問題に対して, 終端拘束ではなく, 終端コストを用いた最適制御問題を扱うことで,計算時間を削減することができる. また, 非線形モデル予測制御

Fig. 1:視覚フィードバックシステム

では, その安定性が特に重要になる. 安定性を確保するためのひとつのアイデアが, 終端コストとしてシステムの CLF (controlLyapunov function) を用いることであり, その閉ループ系は安定となる 2) . この CLF を用いる手法は Caltech Ducted Fanや, F-16 戦闘機などのアプリケーションへ応用されている 3,4,5) .

予測制御を提案し, 第 4 章に検証実験を示す. 最後に本稿をまとめる.

一方, 比較的高速なシステムのひとつとして, 視覚フィード2

動的 2 次元視覚フィードバックシステム

バックシステムがある. 視覚フィードバックシステムは, 視覚情報をフィードバックループに組み込んだ制御であり, ロボットをリアルタイムで制御することから, 化学プラントなどに比べて非常に高速なシステムとして捉えることができる. 文献6)

2.1

2 次元視覚フィードバックシステム

本稿で用いるマニピュレータダイナミクスはにおい

M (q)q̈ + C(q, q̇)q̇ + g(q) = τて, 筆者らは 2 次元視覚フィードバックシステムに対する受動性に基づく制御を提案している.

(1)で表される. q, q̇, q̈ ∈ Rn はそれぞれ各関節の角度, 角速度, 角

本稿では, このような高速なシステムである 2 次元視覚フィー加速度を表し, τ はトルクである. M (q) ∈ Rn×n は正定な慣性ドバックシステムに対する安定化モデル予測制御を提案する. ま

行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項, g(q) ∈ Rn は重た, Fig. 1 に示す SICE-DD アームを用いた 2 次元視覚フィー力項を表す.

ドバックシステムに提案した制御則を適用し, 検証実験をおこな

一方, 視覚フィードバックシステムでは, カメラモデルを考慮すう. そして, 従来法である受動性に基づく制御則を適用した場合る必要がある. 本システムは Fig. 2 に示すように, カメラがマニとの比較をおこなう.

ピュレータの手先に取り付けられた Eye-in-Hand 構造とする. こ

本稿の構成は以下のとおりである. 第 2 章では, 本稿で扱う

こで, マニピュレータの台座を基準座標系 Σw = {Xw Yw Zw }視覚フィードバックシステムについて述べる. 第 3 章でモデルとし, カメラ座標系, 画像座標系をそれぞれ Σc = {Xc Yc Zc },Yw

ただし, η := JbT Rwc f である. α は正定な行列であり, u はモデr
Manipulator 2Sw述べる. この制御トルクを用いることで, 動的 2 次元視覚フィーlZc

q2

r1q1

ル予測制御によって決定される制御入力であり, 詳細は後の章でXc

CameraYcSc

ドバックシステムは式 (1), (4), (7) より, 次のように表される.
8−1< ξ̇ = −M −1 (q)C(q, q̇)ξ„+ M (q)u«sλ Tsλ TTRwc Jb ξ −Rwc Jb αJbT Rwc + RwcṘwc f: f˙ = −zwozwo

Image Plane
Xil2 Yil1Si(8)Xw

ただし, ξ = q̇ − αη である. 本稿の目的は, この動的 2 次元視覚f

フィードバックシステム (8) にモデル予測制御を適用すること

TargetObjectである.

2.3

Zw

受動性に基づく制御

従来研究 6) では, 動的 2 次元視覚フィードバックシステム(8) に対して, 以下のようなエネルギー関数 V (x) によって, 漸Fig. 2: 2 次元視覚フィードバックシステムの概略図

近安定性が保証されている.

V (x) =Σi = {Xi Yi } とする. 基準座標系からみて, カメラは pwc =[xwc (q) ywc (q)]T の Xw − Yw 平面上に取り付けられており, 観T

測対象は, Xw − Yw 平面上の pwo = [xwo ywo ]から Zw 方向

道に沿って時間微分すると,に zwo だけ離れた点に位置する. 本稿では, 観測対象は常に静により, 次のように記述される.
»–sλ Tfx
Rwc (θwc (q)) (pwo − pwc )f (q) :=
=fyzwoTTJb ξ − kp f T Rwc
Jb αJbT Rwc f
V̇ = ξ T u − kp f T Rwc

ただし, s > 0 は単位 pixel/m のスケーリング係数, λ はカメラの焦点距離, Rwc は回転行列を表している

7)

TTJb ξ − kp αf T Rwc
Jb αJbT Rwc f
V̇ = −ξ T Rξ − kp f T Rwc. マニピュレータ

ṗwc (q) := Jb (q)q̇

(3)

したがって, カメラモデルのダイナミクスは以下のようになる.

sλ TTf˙ = −R Jb q̇ − Rwc
Ṙwc fzwo wc(4)

本稿で用いる回転行列 Rwc およびマニピュレータヤコビアン

Jb は以下で与えられる.
–»cos (q1 + q2 ) − sin (q1 + q2 )(5)
Rwc =cos (q1 + q2 )sin (q1 + q2 )–»−l1 sin q1 − l2 sin (q1 + q2 ) −l2 sin (q1 + q2 )(6)
Jb =l2 cos (q1 + q2 )l1 cos q1 + l2 cos (q1 + q2 )
2.2

(11)

ただし, R は正定な行列である. このとき, 式 (10) より,= −xT Qxの角速度 q̇ とカメラ速度 ṗwc (q) は, マニピュレータヤコビアン

Jb (q) ∈ Rn×n を用いて次のように表される.

(10)となる. ここで, 制御入力 u = up を , 以下として考える.

up = −Rξ(2)(9)

ただし, kp > 0 はスカラーである. V (x) をシステム (8) の解軌止しており, ṗwo = 0 とする. ここで, 透視変換を用いてカメラモデルを導出する. pco の画像特徴量 f (q) は Σi への透視変換

kp zwo T1 Tξ M (q)ξ +f f22sλ(12)となる. ただし, 状態 x, 行列 Q はそれぞれ次のように表される.
”T 321 “ T»
–kp Rwc JbRξ25(13)
x=, Q=4 1TTfkp Rwc
Jb kp Rwc
Jb αJbT Rwc
2
ここで, 行列 R を次式で表す.
”T “”−1
1 “ TTTRwc
Jb
Jb αJbT Rwc
Rwc
Jb + KR = kp Rwc
41(14)= kp α−1 + K > 04
ただし, K は正定な行列である. このとき, 行列 Q は正定となり, 式 (12) より, V̇ < 0 が満たされる. したがって, システムは漸近安定となることがわかる.

動的 2 次元視覚フィードバックシステム

本稿では, ロボットダイナミクス (1) と視覚フィードバック3

動的 2 次元視覚フィードバックシステムに対する安定化モデル予測制御

システム (4) を同時に考慮する動的 2 次元視覚フィードバックシステムを考える. 我々の制御目的は, マニピュレータの手先に取り付けられたカメラが, 観測対象のちょうど真上に位置するように制御することである. このとき, 画像特徴量 f はゼロとなる. 制御目的を達成するために, 以下のような制御トルクを提案する.

この章では, 動的 2 次元視覚フィードバックシステムに対する安定化モデル予測制御を提案する. まず, 前章で述べたエネルギー関数を利用することで, 終端コストとして用いる CLF を導出する. そして, その CLF に対する正定な関数とともに評価関数を構成することで, モデル予測制御を用いた閉ループ系が安定

τ = M (q)αη̇ + C(q, q̇)αη + g(q) + u(7)となることを示す.

3.1

モデル予測制御の問題設定

今, サンプリング周期を δ [s] とする. システム (8) に対し,ある時刻 t において, 次式で与えられる評価関数を最小とする入力 u を求める最適制御問題を考える.
Z t+Tl(x(τ ), u(τ ))dτ + M (x(t + T ))J(u, t) =証明 1 式 (18), (22) より, 評価関数 (21) は次のようになる.
Z t+T
J(u, t) =l(x(τ ), u(τ ))dτ + 4V (x(t + T ))t

Z t+T “”u + 2Rξ2R−1 − 4V̇ dτ + 4V (x(t + T ))=t

Z t+T(15)t=ここで求められた入力が, δ [s] 間だけ系に加えられる. 次のサンプリング周期には, 次の時刻を t とした最適制御問題を解いて得t系の入力がモデル予測制御の制御入力となる. 次節では, この(24)

したがって, u = −2Rξ とすると, 評価関数の最小値J ∗ (t) = 4V (x(t))

られる入力を適用する. これを繰り返すことで得られる閉ループ8)

u + 2Rξ2R−1 dτ + 4V (x(t))(25)

最適制御問題を与える評価関数の終端コストとして, 動的 2 次が得られる. 今, J ∗ (t) をリアプノフ関数候補として, 動的 2 次

元視覚フィードバックシステムの CLF を用いることを考える.

元視覚フィードバックシステム (8) の解軌道に沿って時間微分

また, このとき得られる閉ループ系が安定となることを示す.

をおこなうと,

3.2

J˙∗ = 4V̇ = −4xT Q x”T 321 “ TkR
J2Rpbwc
25Q := 4 1TTkp Rwc
Jb kp Rwc
Jb αJbT Rwc
2

動的 2 次元視覚フィードバックシステムに対する安定化モデル予測制御

ここで, CLF を以下のように定義する.
定義 1 8) 正定な関数 V (x) が以下の式を満たすとき, V (x) はi

R=inf V̇ + l(x, u) ≤ 0(16)u

ただし l(x, u) はある正定な関数である.

(27)となる. ところで,CLF である.
h(26)1kp α−1 + K
4(28)であり, α > 0 であることから, Q > 0 が成り立つ. したがって,J˙∗ < 0 となり, 閉ループ系が漸近安定であることが示された.

この条件 (16) を満たす正定な関数 l(x, u) とそれに対する CLF
定理 1 は, 動的 2 次元視覚フィードバックシステムの CLF

V (x) によって評価関数を構成することを考える.
ここで, 準備として, 式 (12) から以下の数式展開を試みる.

を終端コストとして用いることで, そのモデル予測制御が閉ループ系を安定とすることを示した. また, モデル予測制御は最適制−4xT Qx = − (u + 2Rξ)T R−1 (u + 2Rξ) + uT R−1 uTTT+4ξ u + 4ξ Rξ − 4ξ Rξ − 4kp f

T御の一種であるため, 評価関数の状態 x に対する重み行列と入TRwc
Jb ξに基づく制御より性能が向上することが見込まれる.

T
Jb αJbT Rwc f−4kp f T Rwc= − u + 2Rξ2R−1 + uT R−1 u + 4V̇(17)) 4V̇ + 4xT Qx + uT R−1 u = u + 2Rξ2R−1(18)したがって, 以下の不等式が成り立つ.
hiinf 4V̇ + l(x, u) ≤ 0(19)l(x, u) := 4xT Qx + uT R−1 u(20)u

力 u に対する重み行列を調整することで, 従来研究 6) の受動性
次章では, 3 章で述べてきたモデル予測制御を用いた実験をおこなう. 受動性に基づく制御と比較することで, その有効性を検証する.

4

実験結果

この章では, Fig. 1 に示す視覚フィードバックシステム対し,受動性に基づく制御を適用した場合と, 前章で提案したモデル予式 (16) より, 上記の l(x, u) を用いると, 4V (x) が動的 2 次元測制御を適用した場合の実験をおこない, 比較をおこなう. 本実験

視覚フィードバックシステム (8) の CLF であることがわかる.

では SICE-DD アームの手先にカメラを取り付けており, 1 軸が上記の CLF 4V (x), 正定な関数 l(x, u) を用いて, 以下の定理が0.2 [m], 2 軸が 0.3 [m] である. カメラは SONY 社 XC-HR57成り立つ.のカメラを使用する. また, 画像入力ボードは, PCI バスモノク

定理 1 動的 2 次元視覚フィードバックシステム (8) に対し, 評

ロ画像入力ボードの PicPort-Stereo-H4D を使用し, 特徴量は価関数を次式とする最適制御問題を考える.
Z t+Tl(x(τ ), u(τ ))dτ + 4V (x(t + T ))J(u, t) =するディジタル制御装置は dSPACE 社の DS1005 (PowerPC

画像処理ソフト HALCON を利用して得られる. 制御則を実装(21)

tl(x, u) := 4xT Qx + uT R−1 u
kp zwo T
1V (x) = ξ T M (q)ξ +f f
22sλ(22)(23)

750 processor, 480 MHz) を用いており, サンプリング周期は20 [ms] である. また, 制御則は MATLAB と SIMULINK により設計し, dSPACE 社のソフトウェアである ControlDesk により DSP に実装する. また, オンラインで最適制御問題を解く

この評価関数を用いたモデル予測制御は, 動的 2 次元視覚フィーために, C/GMRES 9) というソフトウェアを用いる. マニピュ

ドバックシステムの安定化制御則となる.

レータの初期状態はそれぞれ, q1 (0) = π/6 rad, q2 (0) = −π/65rad, q̇1 (0) = q̇2 (0) = 0 rad/s である. 観測対象は, カメラの初T期位置から見て f (0) = [−120 − 160]の位置に設定する. 我々の制御目的はカメラを観測対象の真上に位置させることであり,このとき, 画像特徴量 f はゼロになる. 設計パラメータは受動性に基づく制御, モデル予測制御のどちらの場合も同じ値を用い,以下のように設定した.

おわりに本稿の目的は, 動的 2 次元視覚フィードバックシステムに安定化モデル予測制御を適用することであった. 従来研究で得られていた動的 2 次元視覚フィードバックシステムのエネルギー関数から CLF を生成した. この CLF を終端コストとして用いることで, モデル予測制御が安定化制御則となることを示した. さ1230
sλ= 1367, kp = 0.13=zwo
0.9
»
–»–
0.05
00.01
0
α=, K=00.15
00.01

(29)

らに, 本稿で提案した安定化モデル予測制御を, 従来の受動性に基づく制御と実験による比較をおこなった. 実験結果から, モデ(30)

ル予測制御が閉ループ系を安定化していることを確認した.

モデル予測制御ではホライズンの長さ T = 0.1 [s] としている.
また本実験では, 外乱として t = 3 s で観測対象の位置をカメラ

参考文献の初期位置から見て f = [−50 − 64]T の位置にずらしている.

1) D. Q. Mayne, J. B. Rawlings, C. V. Rao and P. O. M.

それぞれの実験で得られた f ならびに ξ の時間応答を Fig.

Scokaert, “Constrained Model Predictive Control: Sta-3 – 6 に示す. それぞれ図中の実線はモデル予測制御を適用した

bility and Optimality,” Automatica, vol. 36, no. 7, pp.

場合, 鎖線は受動性に基づく制御を適用した場合の結果を表して

789–814, 2000.

いる. 実験結果より, モデル予測制御を適用した場合でも, システムが安定となっていることがわかる. また, 受動性に基づく制御を適用した場合と比較すると, より平衡点近傍に収束している

fx [pixel]

ことがわかる.

2) A. Jadbabaie and J. Hauser, “On the Stability of Unconstrained Receding Horizon Control with a General Terminal Cost,” Proc. 39th IEEE Conference on Decisionand Control, pp. 4826–4831, 2001.

1003) J. Yu, A. Jadbabaie, J. Primbs and Y. Huang, “Compar-0ison of Nonlinear Control Design Techniques on a Model-100of the Caltech Ducted Fan,” Automatica, vol. 37, no. 12,-2000123time [s]456pp. 1971–1978, 2001.
4) A. Jadbabaie and J. Hauser, “Control of a Thrust-Fig. 3: fx の時間応答

vectored Flying Wing: a Receding Horizon – LPV Ap-yf [pixel]

praoch,” International Journal of Robust and Nonlinear
100Control, vol. 12, pp. 869–896, 2002.

05) R. Bhattacharya, G. J. Balas, M. A. Kaya and A.

-100-200Packard, “Nonlinear Receding Horizon Control of an F0123time [s]456ics, vol. 25, no. 5, pp. 924–931, 2002.

Fig. 4: fy の時間応答ξ [rad/s]

16 Aircraft,” Journal of Guidance, Control, and Dynam-6) 丸山, 藤田, “ロバスト視覚フィードバックシステムの L2 ゲインによる制御性能解析,” 電気学会論文集 C, vol. 118, no.

53, pp. 301–309, 1998.
0-507) M. Fujita, A. Maruyama, M. Watanabe and H. Kawai,
123time [s]456“Inverse Optimal H∞ Disturbance Attenuation for Planar Manipulators with the Eye-in-Hand System,” Proc.
39th IEEE Conference on Decision and Control, pp.

Fig. 5: ξ1 の時間応答ξ [rad/s]

3945–3950, 2000.
58) A. Jadbabaie, J. Yu and J. Hauser, “Stabilizing Receding
Horizon Control of Nonlinear Systems: A Control Lya-0punov Function Approach,” Proc. 1999 American Con-50123time [s]4Fig. 6: ξ2 の時間応答

56trol Conference, pp. 1535–1539, 1999.
9) T. Ohtsuka, “A Continuation/GMRES Method for Fast
Computation of Nonlinear Receding Horizon Control,”Automatica, vol. 40, no. 4, pp. 563–574, 2004.