入力と状態に上下限制約のあるモデル予測制御
に対するセミスムーズニュートン法
田地 宏一
1鈴木 脩平
2概 要
In model predictive control (MPC), an optimal control problem is solved at each time steps to determine control input. To realize on-line control of MPC, reducing computational time is requisite. In this paper, we apply a semismooth Newton method for MPC with simple bounds. The semismooth Newton method is one of iterative methods and is used to solve complemen-tarity problems and KKT systems of optimization problems. The semismooth Newton method has an advantage over other QP solvers, such as interior point methods, active set methods and so on, that the initial point can be chosen arbitrarily, and this enables hot start. We show that the proposed method is globally convergent. We also show the condition guaranteeing the nonsin-gularity of the generalized Jacobian at a solution, which is closely related to the quadratic convergence of the algorithm. This is the first result to clar-ify the reason why constraints on state variables make MPC difficult from the algorithmic perspective. Furthermore, we apply the proposed method to the so-called soft constrained problems, in which the bound constraints for state variables are replaced by quadratic penalty function. Some numerical examples show that the proposed method is practically efficient.
1
はじめに
モデル予測制御 (Model Predictive Control：MPC) とは，時刻と共に移動する予 測区間内での未来を予測し，評価関数を最小にするような最適な入力を計算し，そ の一部を入力として用いることを繰り返す制御手法である．各時刻ごとに最適制御 問題を解かなければならず，予測区間を大きくするにつれ，計算時間が増大してし まう．そのため，ロボットや自動車などの応答の速いシステムに適用し，オンライ ンで制御を行うためには，計算時間の短縮化が必要となる． これまでに，モデル予測制御に対して，さまざまな高速解法が提案されている [2][11][14][15]．Rao ら [11] は，入力及び状態に拘束のある MPC 問題に内点法を適 用した．内点法では各反復においてニュートン方向を求めるために線形方程式を解 く必要があるが，再帰的な関係式を利用することで，線形方程式を予測区間に比例 する計算量で解く手法を提案した．また，Wang ら [15] も入力と状態に拘束のある MPC問題に対し内点法を適用し，線形方程式をブロック LDL 分解で解くことで， 1名古屋大学大学院工学研究科 機械理工学専攻（電子機械工学分野）〒 464-8603 名古屋市千種区 不老町 [email protected] 2名古屋大学大学院工学研究科 機械理工学専攻（電子機械工学分野）現 住友軽金属
計算量を予測区間の線形に抑えた．Dimitrov ら [2] はヒューマノイドロボットの歩 行運動の生成のために状態，入力拘束のある MPC に対してアクティブセット法を 適用した．さらに Dimitrov らは，変数変換を行うことで，一般的な不等式制約を単 純な上下限のある不等式制約にできることを示している． ところで，MPC を適用するようなシステムにおいては，一時刻あとのシステムの 状態は現在の状態に近いと考えられる．このため，MPC において一時刻進んだ後の 制御入力を求める問題では，現在の状態と最適制御入力が非常によい近似解となっ ていることが期待でき，ホットスタートが可能な手法は MPC において有効に働く と考えられる．この点に着目し，田地ら [14] は入力に上下限制約のある MPC 問題 を線形相補性問題に帰着させ，セミスムーズニュートン法 [7] を用いた解法を提案し た．セミスムーズニュートン法は相補性問題や KKT 条件などを解く際に利用され る反復解法であり，大域的収束し二次収束する効率のよい解法の一つである．さら に，反復の初期点を自由に選ぶことができるので，初期解を実行可能内点に選ばな ければならない内点法や，アクティブな制約を特定しなければならないアクティブ セット法よりも有利になることが期待できる． 本論文では，入力と状態に上下限制約のある MPC 問題に対し，セミスムーズニュー トン法を用いた高速解法を提案する．提案手法は，線形相補性問題の定式化を用い た田地ら [14] の手法を状態制約のある問題に拡張したものであり，線形方程式の解 法に Rao らの再帰的な関係式やブロック LDL 分解を用いることで計算の高速化を 行う．提案手法は，最適化問題が実行可能であれば，大域的に収束することを証明 し，また，二次収束性を保証するための解での一般化ヤコビ行列の正則条件を導出 した．とくに，解において状態制約がすべて非アクティブであれば常に一般化ヤコ ビ行列が正則であることを示した．このことは，MPC において状態制約が困難を 生ずることの一つの理論的な証拠とみることができる．さらに，この結果に基づき， 状態変数に対する制約を二次のペナルティ関数で置き換えた，いわゆるソフト制約 問題に対し提案手法を適用した． 本論文の構成は，以下の通り．まず 2 節で今回扱う状態と入力に上下限制約のあ るモデル予測制御問題の定式化を行う．3 節でセミスムーズニュートン法を説明し， 大域的収束定理と一般化ヤコビ行列の正則定理を述べる．4 節では，ソフト制約問 題へ適用した場合について述べる．最後に 5 節で数値実験の結果を示すとともに， MCP関数 [5] を用いた高速化についても述べる．
2
問題の定式化
入力と状態に上下限制約をもつシステムに対し，MPC を適用することを考える． このとき各時刻で次のような最適制御問題を解いて制御入力 ukを計算する．問題 1 min 1 2x T k+NQfxk+N+ k+N∑−1 t=k 1 2(x T tQxt+uTtRut) s.t. xt+1 = Axt+ But(t = k,· · · , k+N −1) x− ≤ xt ≤ x+(t = k +1,· · · , k+N) u− ≤ ut≤ u+(t = k,· · · , k+N −1) ここで，k は時刻，N は予測区間の長さを表し，xt ∈ Rn，ut ∈ Rmはそれぞれス テップ t での状態と入力である．また，R は正定値対称行列であり，Q は半正定値対 称行列，Qf は半正定値対称行列で，例えば，離散形リカッチ方程式の解を用いる． 状態の上下限 x+, x−∈ Rnおよび入力の上下限 u+, u−∈ Rmは定数とする． vt ∈ Rnを状態方程式 xt+1 = Axt+ Butに対するラグランジュ乗数，gt, ht ∈ Rn を状態の上下限制約，wt, zt ∈ Rmを入力の上下限制約に対するラグランジュ乗数と すると，この最適化問題の KKT 条件は次のように書ける． Rut+ BTvt+ wt− zt= 0 (1a) Axt+ But− xt+1 = 0 (1b) wt ≥ 0, −ut+ u+ ≥ 0, wTt(−ut+ u+) = 0 (1c) zt≥ 0, ut− u− ≥ 0, ztT(ut− u−) = 0, (t = k, k + 1,· · · , k + N − 2) (1d) gt+1 ≥ 0, −xt+1+ x+ ≥ 0, gTt+1(−xt+1+ x+) = 0 (1e) ht+1≥ 0, xt+1− x− ≥ 0, hTt+1(xt+1− x−) = 0, (t = k + 1,· · · , k + N − 1) (1f) Qt+1xt+1+ ATvt+1− vt+ gt+1− ht+1= 0, (t = k + 1,· · · , k + N − 2) (1g) Qk+Nxk+N − vk+N−1+ gk+N − hk+N = 0 (1h) ここで相補性条件 (1c)-(1f) に対して，次式で定義される Fischer-Burmeister 関数 （FB 関数）[6] を導入する． φ(a, b) = a + b−√a2 + b2 (2) 簡単な計算により φ(a, b) = 0⇔ a ≥ 0, b ≥ 0, ab = 0 となることがわかるので， qt= (ut, vt, wt, zt, gt+1, ht+1, xt+1)
とすれば，KKT 条件 (1) は非線形方程式 F (q) = 0 と等価である．ただし，q = (qTk,· · · , qTk+N−1)T，F = (FkT,· · · , Fk+NT −1)T であり，F の各要素 Ft(qt), (t = k, k + 1,· · · , k + N − 1) は Ft(qt) = Rut+BTvt+wt−zt Axt+But−xt+1 φ(wt,−ut+ u+) φ(zt, ut− u−) φ(gt+1,−xt+1+ x+) φ(ht+1, xt+1− x−) Qtxt+1+ATvt+1−vt+gt+1−ht+1 (3) で表される．なお，t = k+N−1 のときの式 (3) の最下列は Qfxk+N−vk+N−1+gk+N−hk+N に置き換わる．また，φ は各要素に FB 関数を持つベクトルである．FB 関数 φ(a, b) は (a, b) = (0, 0) において微分不可能となることに注意する．
3
セミスムーズニュートン法
セミスムーズニュートン法は変分不等式や相補性問題などを解く際に利用される 反復法である．滑らかな方程式に対するニュートン法の一般化であり，速い収束を 意味する 2 次収束の特徴を持つ．また，ノルム関数への直線探索を組み合わせるこ とで大域的収束性を持つ．関数 G : Rn → Rnは局所リプシッツ連続であると仮定 すると Rademacher の定理より，G はほとんど至るところで微分可能である．G が 微分可能な点の集合を DGと書くと，以下に示す B 劣微分や一般化ヤコビ行列を定 義できる [1][3]． 定義 3.1 局所リプシッツ連続な関数 G : Rn → Rnに対し，以下で定義される集合 ∂BG(x)を x での B 劣微分という． ∂BG(x) = { V ∈ Rn×n|V = lim xi→x,xi∈D G G0(xi) } さらに，∂BG(x)の凸包を Clarke の一般化ヤコビ行列とよび，∂G(x) とかく． 2 Gが x で微分可能であれば，∂BG(x)や ∂G(x) はいずれもヤコビ行列 G0(x)に一 致する．FB 関数 (2) の一般化ヤコビ行列は次式で表される． ∂φ(a, b) = (d(a, b), e(a, b))Tだたし d，e はそれぞれ ( d(a, b) e(a, b) ) = {( 1− ξ 1− η ) ξ2+ η2 ≤ 1 } if (a, b) = (0, 0) ( 1− √ a a2+b2 1− √ b a2+b2 ) if (a, b)6=(0, 0)
である．次に，セミスムーズニュートン法の収束率と関係のあるセミスムーズ性を 定義する [8][10]． 定義 3.2 局所リプシッツ連続な関数 G : Rn → Rnに対し，任意の V ∈ ∂G(x + h) とkhk → 0 において V h− G0(x; h) = o(khk) を満たすとき，x においてセミスムーズ（semismooth）であるという．さらに， V h− G0(x; h) = O(khk2) を満たすならば G は強セミスムーズ（strongly semismooth）であるという．ここで G0(x; h)は方向微分である． 2 セミスムーズニュートン法では，各反復において，次の線形方程式の解を求め，点 列 xkを生成する． xk+1 = xk− Vk−1G(xk) ただし，Vk ∈ ∂BG(kk)または Vk∈ ∂G(xk)である．連続的微分可能な関数や，線形 関数は強セミスムーズである．また，FB 関数も強セミスムーズである．さらに関数 (φ(a, b))2は連続的微分可能である [4]．
3.1
アルゴリズムと大域的収束性
F (q)は一次関数と FB 関数で構成されているので，リプシッツ連続かつ強セミス ムーズある．さらに θ(q) = 12kF (q)k2は連続的微分可能であり，その勾配は一般化 ヤコビ行列 V ∈ ∂F (q) を用いて ∇θ(q) = VTF (q)となる [7]．そのため θ(q) をメリッ ト関数として用いることにより，大域的に収束するセミスムーズニュートン法を構 成することができる．アルゴリズムは次のようになる． アルゴリズム step 0 初期点 q0と定数 ρ > 0，p > 2，σ∈ (0, 12)を与える．k = 0． step 1 停止基準を満たすならば終了し，qkを解とする． step 2 Vk ∈ ∂F (qk)を計算し，次の方程式の解 dkを求める． Vkdk=−F (qk) (4) (4)に解がないか，条件，∇θ(qk)Tdk≤ −ρ k dk kpを満たさないならば，dk = −∇θ(qk)とする．step 3 次を満たす最小の非負の整数 i を求める． θ(qk+d k 2i ) ≤ θ(qk) + σ 2i∇θ(q k)Tdk qk+1 = qk+ 2−idk，k ← k + 1 として，step 1 へ． アルゴリズムの大域的収束性を述べるため，以下の記号を導入する．まず，Ww t , Wu t, Ztz, Ztu ,G g t, Gxt ,Hth, Htxを定義する．これらは対角行列であり，その (i, i) 成 分は以下のようになる． ( Ww i Wu i ) = ∂φ(wi,−ui+ u+) = ( −d(wi,−ui+ u+) e(wi,−ui+ u+) ) ( Ziz Zu i ) = ∂φ(zi, ui− u−) = ( d(zi, ui− u−) e(zi, ui− u−) ) ( Ggi Gx i ) = ∂φ(gi,−xi+ x+) = ( −d(gi,−xi+ x+) e(gi,−xi+ x+) ) ( Hih Hx i ) = ∂φ(hi, xi− x−) = ( d(hi, xi− x−) e(hi, xi − x−) ) さらに，以下の記号を定義する． Ad:= I 0 · · · 0 −A I 0 0 −A I . .. ... .. . . .. ... ... 0 0 · · · 0 −A I Bd:= diag{B, B, · · · , B| {z } N個 }, Qd:= diag{Q, · · · , Q| {z } N−1 個 , Qf}, Rd:= diag{R, R, · · · , R| {z } N個 } X := x1 x2 .. . xN , U := u1 u2 .. . uN , X0 := Ax0 0 .. . 0 これらの記号を用いると，問題 1 は min XTQdX + UTRdU s.t. AdX + BdU = X0 X− ≤ X ≤ X+, U− ≤ U ≤ U+
と書き直すことができる．前節と同様にして FB 関数を用いると，この問題の KKT 条件と等価な非線形方程式 ˜ F (q) = F1 F2 F3 F4 F5 F6 F7 = QdX + ATdV + G− H RdU + BdTV + W − Z X0− AdX− BdU Φ(X+− X, G) Φ(X − X−, H) Φ(U+− U, W ) Φ(U − U−, Z) = 0 (5) が得られる．ここで Φ(X+− X, G) は φ(x+ − xt, gt)を並べたものであり，Φ(X − X−, H)，Φ(U+− U, W )，Φ(U − U−, Z)も同様に定義される．さらに， ˜F の一般化 ヤコビ行列は ˜V は以下のようになる． ˜ V = Qd 0 ATd I −I 0 0 0 Rd BdT 0 0 I −I −Ad−Bd 0 0 0 0 0 Gx 0 0 Gg 0 0 0 Hx 0 0 0 Hh 0 0 0 Wu 0 0 0 Ww 0 0 Zu 0 0 0 0 Zz (6) Gxは対角成分に Gx t を持つ対角行列であり，Gg, Hx, Hh, Wu, Ww, Zu, Zzも同様 である．方程式 (5) は方程式 F (q) = 0 の式と変数の順序を並び替えたものであるか ら，以下では F と ˜F，およびそれぞれの一般化ヤコビ行列 V ∈ ∂F (q) と ˜V ∈ ∂ ˜F (q) を同一視する． ここで，大域的収束定理を述べる． 定理 3.1 行列 Q と Qf は半正定値，R は正定値と仮定する．このアルゴリズムに よって生成される点列{qk} のすべての集積点 q∗は θ(q) の停留点であり，F (q) = 0 の解である． 証明: ∇θ(q∗) = 0となることは，[3] の Th 9.1.11 よりしたがうので，ここでは， ∇θ(q∗) = 0ならば F (q∗) = 0となることを示す．∇θ(q) は一般化ヤコビ行列の任意 の要素 ˜V ∈ ∂ ˜F (q)を用いて∇θ(q) = ˜VTF (q)˜ と表されるので，式 (5) より∇θ(q∗) = 0 ならば QdF1− ATdF3+ GxF4+ HxF5 = 0 (7a) RdF2− BdTF3+ WuF6+ ZuF7 = 0 (7b) AdF1+ BdF2 = 0 (7c)
F1+ GgF4 = 0 (7d)
−F1+ HhF5 = 0 (7e)
F2+ WwF6 = 0 (7f)
−F2+ ZzF7 = 0 (7g)
である．FB 関数に関する簡単な計算により，0 ≤ d(a, b), e(a, b) ≤ 2 であること，
および d(a, b) = 0 または e(a, b) = 0 ならば φ(a, b) = 0 となることに注意してお く．すると，たとえば (7d) 式の第 i 式において，(Gg)i = −d(gi,−xi+ x+) = 0な らば (F4)i =−φ(gi,−xi+ x+) = 0となる．そのとき，対応する (7e) 式の第 i 式は， (Hh) i(F5)i = d(hi, xi− x−)φ(hi, xi− x−) = 0となるが，これは (F5)i = 0を意味す る．同様の関係は，式 (7f) と (7g) についても成り立つので，一般性を失うことなく Gg，Hh，Ww，Zzの対角成分は非ゼロであると仮定してよい． したがって，式 (7c)，(7d)，(7e)，(7f)，(7g) より， F1T × 式 (7a) + F2T × 式 (7a) = F1T(QdF1− ATdF3+ GxF4+ HxF5) + F2T(RdF2− BdTF3+ WuF6+ ZuF7) = F1T (Qd− Gx(Gg)−1+ Hx(Hh)−1 ) F1+ F2T ( Rd− Wu(Ww)−1+ Zu(Zz)−1 ) F2 = 0 (8) となる．対角行列 Gg，Wwの対角成分は負となることに注意すると式 (8) より F 1 = 0， F2 = 0が得られ，式 (7d)，(7e)，(7f)，(7g) より F4 = F5 = F6 = F7 = 0となる．し たがって，式 (7a) より ATdF3 = 0となるが，Adは正則であるので F3 = 0を得る．2
3.2
一般化ヤコビ行列と二次収束性
アルゴリズムの二次収束性は以下の定理によって保証される． 定理 3.2 q∗を F (q) = 0 の解とする．F が q∗において強セミスムーズかつ BD 正則， すなわち，すべての V ∈ ∂BF (q∗)が正則であれば，二次収束する． 2 定義 3.1 より ∂BF (q)⊂ ∂F (q) が成り立つので，ここでは解における一般化ヤコビ 行列のすべての要素 V ∈ ∂F が正則となる条件を導く．まず，解において変数は上限 と下限の値を同時にとることはないことに注意すると，適当に符号を換えることに より，すべての上限制約は非アクティブとしてよい．このとき，Gg =−I，Gx = 0，Ww =−I，Wu = 0となるので，式 (6)の ˜V の正則性は ¯ V = Qd 0 ATd −I 0 0 Rd BdT 0 −I −Ad−Bd 0 0 0 Hx 0 0 Hh 0 0 Zu 0 0 Zz の正則性に帰着できる．さらに，次の添字集合を定義する．
α ={i|Hih = 0}, ¯α = {i|Hih > 0}, β = {i|Ziz = 0}, ¯β = {i|Ziz > 0}
定義より，Hh i = 0，Ziz = 0となるのは，それぞれ xi = x−，ui = u−の場合に限る ことに注意する．添字集合に合わせて分割すると， ¯V は Qd 0 ATd −Iα−Iα¯ 0 0 0 Rd BTd 0 0 −Iβ−Iβ¯ −Ad−Bd 0 0 0 0 0 Hx α 0 0 0 0 0 0 Hαx¯ 0 0 0 Hαh¯ 0 0 0 Zu β 0 0 0 0 0 0 Zu ¯ β 0 0 0 0 Z z ¯ β となる．行列 Hαh¯ と Zβz¯に関する Schur complement をとると ˜ Qd 0 ATd −Iα 0 0 R˜d BTd 0 −Iβ −Ad−Bd 0 0 0 Hαx 0 0 0 0 0 Zu β 0 0 0 となる．ただし，( ˜Qd)ii= Qii+ (Hih)−1Hix, i∈ ¯α であり，( ˜Rd)ii = Rii+ (Ziz)−1Ziu, i∈ ¯β である．この行列が正則となる十分条件の一つは， [ ˜ Qd 0 0 R˜d ] が正則，かつ [ ATd −Jα 0 BT d 0 −Jβ ] (9) が列フルランクとなることであるが，行列 R は正定値対称であることに注意すると 以下の命題を得る．
命題 3.1 行列 Q，Qf が正定値対称，かつ行列 [ATd − Jα] が列フルランクであれば，解における一般化ヤコビ行列 ˜V のすべての要素は正則で ある． 2 とくに，解においてすべての状態制約が非アクティブ，X− < X∗ < X+のときに は，α = φ となる．このとき，Adが正則であることに注意すると，上の証明を少し 修正することで次の定理を得る． 定理 3.3 Q，Qf は半正定値対称行列，R は正定値対称行列であるとする．問題１ の解においてすべての状態制約が非アクティブ X− < X∗ < X+のとき， ˜F (q)の一 般化ヤコビ行列のすべての要素 ˜V は正則である． 2 補足 3.1 モデル予測制御においては，一般に状態制約があると難しいといわれてい た．解における一般化ヤコビ行列の正則性は，アルゴリズムの二次収束性と直接関 係していることから，上の定理は，状態制約が MPC を難しくする理由を計算アル ゴリズムの観点から明らかにした初めての結果である．
4
ソフト制約問題への応用
モデル予測制御においては，制御入力を決める問題 1 が実行不能となることが起 こりえる．通常のシステムでは，制約条件に対してある程度のマージンをとってあ ると考えられるため，制約条件を多少逸脱してもシステムは稼働できることが多い． そこで，一つの方法として制約を緩和するいわゆるソフト制約 [9][13] がある．ソフ ト制約では，状態または入力の制約を緩和するが，本論文では前節の結果に基づき， 状態制約を二次のペナルティ関数に置き換える．`2-ペナルティ関数を用いたソフト 制約の最適制御問題は次のようになる． 問題 2 min 1 2x T k+NQfxk+N+ k+N∑−1 t=k 1 2(x T tQxt+uTtRut) + ρC(x) s.t. xt+1 = Axt+ But (t = k,· · · , k+N −1) u− ≤ ut≤ u+ (t = k,· · · , k+N −1) ここで，C(x) は以下で定義される x− ≤ x ≤ x+に対する二次のペナルティ関数 C(x) = 1 2 k+N∑−1 t=k {max(0, xt− x+)2 + max(0, x−− xt)2}であり，ρ≥ 0 はペナルティパラメータである．ρ = 0 のときは状態制約なしと等価 であり，ρ を十分大きくとるとハード制約の問題 1 となることに注意しておく．C(x) は 1 階連続的微分可能であるが 2 回微分可能ではないため，問題 2 をセミスムーズ ニュートン法で解くことを考える．問題 2 は前節と同様に， min XTQdX + UTRdU + ρC(X) s.t. AdX + BdU = X0, U− ≤ U ≤ U+ と書き直すことができる．この KKT 条件と等価な非線形方程式は ˆ F (q) = QdX + ρ∇C(X) + ATdV RdU + BdTV + W − Z X0− AdX− BdU Φ(U+− U, W ) Φ(U − U−, Z) = 0 となる．ここで，∇C(X) の第 i 要素は ∇Ci(X) = max(0, xi− x+)− max(0, x−− xi) である．さらに， ˆF の一般化ヤコビ行列は， Qd+ ∂2C(X) 0 ATd 0 0 0 Rd BdT I −I −Ad −Bd 0 0 0 0 Wu 0 Ww 0 0 Zu 0 0 Zz (10) となる．∂2C(X)は対角行列であり，その (i, i) 成分 ∂2Ci(X)は ∂2Ci = 1 (xi < x− または x+< xi) [0, 1] (xi = x− または x+) 0 (x−< xi < x+) (11) である． アルゴリズムの大域的収束性は，定理 3.1 と同様に示すことができる．また，∂2C は非負の対角行列であることに注意すると，(10) の正則性は前節の α = φ の場合に 帰着できる．したがって，以下の収束定理を得る． 定理 4.1 Q，Qf が半正体値対称行列，R が正定値対称行列とする．`2-ソフト制約 問題（問題 2）に対するセミスムーズニュートン法は大域的に収束し，収束率は二 次である．
5
数値実験
本節では計算実験の結果を示す．計算実験は，Intel Core2Quad Q9400 2.66GHz， 3.21GBメモリを持つ計算機上で MatlabR2012a を使用して行った．アルゴリズムで 用いられるパラメータは，ρ = 10−8, p = 2.1, σ = 10−3とし，終了条件は θ(q) < 10−7 とした．実験は以下の５つの方法に対して行った．はじめの３つはセミスムーズニュー トン法に基づく提案手法であり，あとの二つは比較のための汎用手法である内点法 とアクティブセット法である． LDL アルゴリズム中の線形方程式 (4) をブロック LDL 分解 [15] 用いて解いた．(4) 式一回あたりの計算量は 9(m + 2n)3N である． Rao (4)式を Rao らの手法 [11] で解いた．計算量は (15n3+ 7mn2+ 3m2n + 9m3)N で，一般には LDL よりも時間がかかる． MCP 上下限制約に MCP 関数 [5] を適用したもの．(4) 式は LDL 分解で解いた．内点法 (IP) およびアクティブセット法 (AS) いずれも Matlab の quadprog 関数を 利用した． 実験結果を示す前に MCP 関数と初期解について説明する．本論文で用いた MCP 関数は FB 関数 (2) を二つ用いて，上下限制約 x− ≤ x ≤ x+に対して以下で定義さ れる [5]． ψ(x, y) = φ(x− x−,−φ(x+− x, −y)) (12) FB関数の性質より関数 ψ もまたセミスムーズである．また簡単な計算により， ψ(x, y) = 0⇐⇒ x− ≤ x ≤ x+ (x− x−)y ≥ 0 (x− x+)y ≤ 0 となることがわかるので，式 (12) を用いると，たとえば，制御入力の上下限制約に 対応する式 (3) の第 3 式と第 4 式 φ(wt,−ut+ u+) = 0, φ(zt, ut− u−) = 0 を一つの式 ψ(ut, zt) = 0
mass1 mass2 1 1
, u
x
x
2, u
2 mass1 mass2 1 1, u
x
x
2, u
2図 1: Two carts positioning system
で置き換えることができる．したがって，上下限制約に対応する式とラグランジュ 乗数を半分に減らすことができ，ψ の一般化ヤコビ行列の計算が FB 関数より少し複 雑になることを考慮しても計算の高速化が期待できる．なお，アルゴリズムの大域 的収束性や，ヤコビ行列の正則性は，前節で述べた定理と全く同一であるので，こ こではその証明等は省略する． サンプリング時間の短いモデル予測制御では各時刻毎の問題の変化が微小である と考えられるので，解の変化も微小であると考えることができる．そのため，一時 刻前の最適解を現時刻における初期解に設定することで効率的に解を求めることが できると考えられる．そこで，LDL, Rao, MCP では次のように k ステップ目の最 適解 q∗(k|t)を k + 1 ステップの初期解 q0 (k+1|t)とした． q0 (k+1|1) q0 (k+1|2) .. . q(k+10 |N−1) q0 (k+1|N) = q(k∗|2) q(k∗|3) .. . q(k∗|N) q(k∗|N) (13) 提案手法の有効性を示すため，図 1 に示すような 2 質量系の位置決め機構の静定 問題 [12] の数値を変更したものに対しシミュレーション実験を行った．連続時間シ ステムの状態方程式は以下で与えられる． ˙x = Acx + Bcu， Ac= 0 0 1 0 0 0 0 1 −0.1 3 0.1 3 − 0.2 3 0.1 3 0.1 2 − 0.1 2 0.1 2 − 0.1 2 , Bc= 0 0 0 0 1 3 0 0 12
このシステムをサンプリング時間 0.02 で離散化したシステムを制御対象とする．評 価関数の重み行列 Q，R は， Q = 4 0 0 0 0 4 0 0 0 0 1 0 0 0 0 1 , R = [ 0.4 0 0 0.6 ] とし，終端の重みは Qf = Qとした．また，初期状態は x0 = [−0.1, 0.25, 0, 0]T と し，目標状態を xf = [0, 0, 0, 0]T とした．入力の上下限制約は，|u1| ≤ 0.15 および |u2| ≤ 0.15 であり，状態量の上下限制約は −0.3 −0.3 −0.1 −0.1 ≤ x1 x2 x3 x4 ≤ 0.3 0.3 0.1 0.1 (14) とおいた． 実験結果を表 1 にまとめる．表中，N は予測区間の長さを表し，#iter. の列の total は反復回数の総和，max は 1 ステップの計算に要した反復回数の最大値を表
す．同様に，CPU time の列の total は計算時間の総和，ave. は 1 ステップあたり の計算時間の平均値，max は 1 ステップに要した計算時間の最大値をそれぞれ秒で 表す．制御時間は 10 秒としたので，それぞれ問題 1 を 500 回解いた結果である． 表より，提案手法はすべて内点法やアクティブセット法よりも高速に MPC 問題 を解いていることがわかる．とくに，予測区間 N = 15 では提案手法すべてにおい て，1 ステップに要する計算時間の最大値がサンプリング時間 0.02 秒を下回ってお り，本論文の実験環境でも実時間制御が実現できていることがわかる．また，線形方 程式の解法により，計算時間は予測区間とほぼ比例していることがわかる．提案手 法同士の計算時間の比較では，短い順に MCP, LDL, Rao となっており，MCP 関数 を用いることで方程式とラグランジュ乗数の数の削減効果が現れている．なお，反 復回数や反復の最大値が LDL, Rao, MCP で異なっているが，これは，線形方程式 (4)の解き方の違いによる数値誤差などの影響と考えられる． 次に，図 2(a)，図 2(b)，図 2(c) に N = 40 に対して LDL を用いた結果を示す．図 2(a)は制御入力の値，図 2(b) は位置，図 2(c) は速度である．図より，入力および速 度の上下限制約を満たしつつ目標状態に制御している様子がわかる． 図 2(d) に N = 40 に対して LDL と内点法を適用したときの各ステップにおける反 復回数の結果を示す．この図と，図 2(a), 2(c) を比較すると，制御入力や状態変数が 上下限値となっているおよそ 2 秒 (100 ステップ) 以内や，制御入力の変動が大きい ところでは提案手法の反復回数が多くなっていることがわかる．またその後，変動 が緩やかになると，反復 1 回で制御入力が求まっており，ホットスタートおよび初
表 1: Numerical results for two cars system
N #iter. CPU time [s]
total max total ave. max
15 539 4 2.8395 0.0057 0.0138 20 661 10 4.2908 0.0086 0.0855 LDL 25 760 8 5.8719 0.0117 0.0736 30 789 9 7.1728 0.0143 0.0919 35 820 9 8.8629 0.0177 0.1124 40 856 10 9.9811 0.0200 0.1121 15 529 3 3.4016 0.0068 0.0165 20 602 6 4.8665 0.0097 0.0524 Rao 25 645 8 6.3203 0.0097 0.0878 30 662 7 7.6680 0.0153 0.0925 35 658 7 8.8471 0.0177 0.0908 40 684 7 10.3632 0.0207 0.1132 15 589 4 2.2567 0.0045 0.0138 20 658 10 3.3927 0.0068 0.0758 MCP 25 754 8 4.6263 0.0093 0.0630 30 790 10 5.7014 0.0114 0.1060 35 822 9 6.7815 0.0136 0.0907 40 855 10 7.9361 0.0159 0.0883 15 1850 5 15.6705 0.0313 0.0389 20 1882 5 16.4083 0.0328 0.0406 IP 25 1927 5 17.6153 0.0352 0.0414 30 1980 5 18.6470 0.0373 0.0454 35 2023 6 19.2361 0.0385 0.0484 40 2167 6 21.4554 0.0429 0.0502 15 531 4 3.4924 0.0070 0.0571 20 731 8 5.7698 0.0115 0.0726 AS 25 1102 12 10.0001 0.0200 0.0892 30 1501 20 15.3451 0.0307 0.1265 35 1946 28 21.2470 0.0425 0.1853 40 2042 36 27.7852 0.0566 0.2320
期解 (13) の有効性が確認できる．一方，ホットスタートの適用が難しい内点法では， 常に 4∼6 回の反復を要しており，このことが計算時間の増加の原因となっている． 実際，Matlab の quadprog では初期解を指定できるため，式 (13) を各ステップの初 期解に指定した実験も行ったが，初期解を指定しなかった場合と全く同一であった．
(a) Control inputs (b) Position
(c) Velocity (d) The number of iterations (N = 40)
図 2: The reseuts for two cars system
次に同じシステムに対してソフト制約を適用した結果を示す．ここでは，状態の 上下限制約を x+ = 0.05，x− = 0.05とした．予測区間 N = 40 の LDL の結果を表 2に示す．また，ペナルティパラメータ ρ に対する状態変数 x4の軌道を図 3(a) に示 す．図において，Hard は制約条件 (14) の下での結果であり，図 2(c) の点線と同じ ものである． 図 3(a) より，ペナルティパラメータが大きくなるにつれ，制約の侵害が小さくな ることが確認できる．また，表 1 と 2 を比較すると，一時刻あたりの平均計算時間 はハード制約（表 1）のほうが長くなるが，一時刻当たりの最大計算時間はソフト 制約のほうが短くなっている．N = 40，ρ = 1 のときの反復回数の結果を図 3(b) に 示す．図より，ハード制約の問題よりも各ステップでの反復回数のばらつきが小さ いことがわかる．
表 2: Numerical results of two cars sysytem (soft constraits)
iteration time[s]
N ρ total max total ave. max
0 1096 5 6.7836 0.0136 0.0291 20 1 1200 5 7.3859 0.0148 0.0297 5 1342 5 8.1494 0.0163 0.0314 10 1489 6 8.9249 0.0178 0.0351 0 1171 6 10.6747 0.0213 0.0436 30 1 1250 6 11.2262 0.0255 0.0509 5 1446 7 12.7871 0.0256 0.0590 10 1622 8 14.2418 0.0285 0.0671 0 1228 6 14.3517 0.0287 0.0655 40 1 1272 6 14.8347 0.0681 0.0681 5 1516 9 17.3575 0.0347 0.0978 10 1690 9 19.3160 0.0368 0.0984
(a) Trajectoris of x4 (b) The number of iterations
最後に，ソフト制約の有効性を示すため，図 4 のような車両を目標軌道に追従さ せる制御問題を考える．連続時間システムの状態方程式は以下で与えられる． ˙x = Acx + Bcu， Ac= 0 0 1 0 0 0 0 1 0 2(Kf+Kr) m − 2(Kf+Kr) mV − 2(lfKf+lrKr) mV 0 2(lfKf−lrKr) I − 2(lfKf−lrKr) IV − 2(l2 fKf+l2rKr) IV Bc= 0 0 0 0 2Kf m 2Kr m 2lfKf I 2lrKr I
入力は前輪の舵角 δf と後輪の舵角 δrである．状態量は x = [y, θ, ˙y, ˙θ]T であり，y は
車両の位置，θ は進行方向に対する車両の角度である．各パラメータの値は表 3 の 値を用いた．また，評価関数に用いる重み行列は以下の通りである． Q = 1 0 0 0 0 1 0 0 0 0 0.1 0 0 0 0 0.1 , Qf = 10 0 0 0 0 10 0 0 0 0 0.1 0 0 0 0 0.1 , R = [ 3.5 0 0 4 ] 予測区間の長さを N = 30 とした結果を図 5 に示す．図 5(a)，図 5(b)，図 5(c) は それぞれペナルティパラメータに ρ = 0, 1, 10 に対する結果である．図中の破線は目 標軌道である．実線は y 方向の状態に対する上下限制約であり，目標軌道±0.1 と設 定した．ρ = 0 は状態制約がない問題と等価であるので，制約から大きく外れてい るが，ρ = 1，ρ = 10 とペナルティを大きくするにつれて，制約違反の割合が減って おり，ρ = 10 ではほとんど状態制約を満たしていることがわかる．また，計算時間 と反復回数の結果を表 4 にまとめる．ペナルティパラメータ ρ が大きくなるにつれ て計算時間が大きくなっていることがわかる． 図 4: Vehicle Model
0 50 100 150 200 250 -1 -0.5 0 0.5 1 step p o s iti o n (a) ρ = 0 0 50 100 150 200 250 -1 -0.5 0 0.5 1 step p o s iti o n (b) ρ = 1 0 50 100 150 200 250 -1 -0.5 0 0.5 1 step p o s iti o n (c) ρ = 10
表 3: Physical parameters of a vehicle model パラメータ 記号 値 (単位) 質量 m 1400(kg) 慣性モーメント I 2500(Nm) 重心からの距離 (前輪) lf 1.02(m) 重心からの距離 (後輪) lr 1.58(m) コーナリングフォース (前輪) Kf 40000(N) コーナリングフォース (後輪) Kr 40000(N) 速度 V 10(km/h)
表 4: Numerical results for a tracking problem
ρ max time total time ave. time max iter. total iter.
0 0.0316 4.4988 0.081 4 552
1 0.0401 5.4289 0.081 5 674
10 0.0417 6.0615 0.080 5 775
参考文献
[1] F.H. Clarke, Optimization and Nonsmooth Analysis, SIAM, Philadelphia, PA, 1990
[2] D. Dimitrov, A. Sherikov and P. Wieber: A sparse model predictive control formulation for walking generation, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2292/2299 (2011)
[3] F. Facchinei and J.S. Pang: Finite-Dimensional Variational Inequality and Complementarity Problems I and II, Springer-Verlag, New York, 2003
[4] F. Facchinei and J. Soares: A new merit function for nonlinear complementarity problems and a related algorithm, SIAM Journal on Optimization, 7, 225/247 (1997)
[5] M.C. Ferris, C. Kanzow and T.S. Munson: Feasible descent algorithms for mixed complementarity problems, Mathematical Programming, 86, 475/497 (1999)
[6] A. Fischer: A special Newton-type optimization, Optimization, 24, 269/284 (1992)
[7] T. De Luca, F. Facchinei and C. Kanzow: A semismooth equation approach to the solution of nonlinear complementarity problems, Mathematical Program-ming, 75, 407/439 (1996)
[8] R. Mifflin: Semismooth and semiconvex functions in constrained optimization, SIAM Journal on Control and Optimization, 15, 957/972 (1977)
[9] N.M.C. Oliveira and L.T. Biegler: Constraint Handling and Stability Properties of Model-Predictive Control, AIChE Journal, 40(7), 1138/1155 (1994)
[10] L. Qi and J. Sun: A nonsmooth version of Newton’s method, Mathematical Programming 58, 353/367 (1993)
[11] C. Rao, S. Wright and J. Rawlings: Application of interior-point methods to model predictive control, JOTA, 99(3), 723/757 (1998)
[12] 斉藤豊，井村順一：入力拘束システムのモデル予測制御に対する M-行列を用い
た高速解法, 計測自動制御学会論文集，40(9)，906/914 (2004)
[13] P.O.M. Scokaert and J.B. Rawlings: Feasibility Issues in Linear Model Predic-tive Control, AIChE Journal, 45(8), 1649/1659 (1999)
[14] 田地宏一，中島巧，細江繁幸：入力拘束のあるモデル予測制御に対する高速解
法，システム制御情報学会研究発表講演会講演論文集，49，505/506 (2005) [15] Y. Wang and S. Boyd: Fast model predicitve control using online optimization,
