皆様こんにちは．18のhiraです．
モデル予測制御(MPC)では何度も制約つき数理最適化を繰り返します．MATLABの線形MPCで使用される数理最適化であるQPKWIK法について備忘録として書いておきます．
QPKWIK法の元論文はこちらのリンクです．
QPKWIK法は線形制約付き二次計画問題の数値解法であり，有効制約法(アクティブセット法)の1つである．ベクトル \bf{x} の二次関数で最小化したいコスト関数を f(\bf{x}) とすると，線形制約付き二次計画問題は適切な行列 G ， \mathcal{A} とベクトル \bf{g} ， \bf{\beta} を用いて次のように表現できる．
\begin{aligned}f(\bf{x}) &= \frac{1}{2}\bf{x}^TG\bf{x}+\bf{g}^T\bf{x}\\\mathrm{s.t.}\quad\bf{\beta} &\leq\mathcal{A}^T\bf{x}\end{aligned}
制約が無い場合， f(\bf{x}) は(正の)二次関数だから最小値となるのは凸点であると直ちに分かる．凸点となる \bf{x} は， f(\bf{x}) の \bf{x} 微分が \bf{0} になる \bf{x} である．まず f(\bf{x}) を微分すると，
\begin{aligned}\frac{\partial f(\bf{x})}{\partial\bf{x}} = G\bf{x} + \bf{g}\end{aligned}
これが 0 になればよいから，
\begin{aligned}0&= G\bf{x} + \bf{g}\\\bf{x} &= -G^{-1}\bf{g}\end{aligned}
である．この \bf{x} が既に制約を満たしていれば \bf{x} はこの時点で最適解であるので求解を終了する．1つ以上の制約に違反している場合は制約を満たすよう解をずらす動作を繰り返し最適解を求めていく．
線形制約がある場合，制約を満たす最適解は制約の許す限界まで元の凸点に近寄らせた方が f(\bf{x}) が小さくなる．そのため最適解は1つ以上の制約の境界線の上にある．QPKWIK法では暫定解が境界線に乗っている制約を有効制約と言い，次のベクトル \bf{a}_i で表される．
\begin{aligned}\Bigl\{\bf{a}_i\;|\;\bf{a}_i\in\mathcal{A}\;\cap\;\bf{a}_i\bf{x}=\bf{\beta}(i)\Bigr\}\end{aligned}
ここで \bf{a}_i は制約行列 \mathcal{A} のある列の列ベクトルである．この \bf{a}_i が与えられたある制約 \bf{a}^T\bf{x}\leq\bf{\beta} の1つに対応する．ここで， \bf{a}_i のみを抜き出して制約行列を再構成する．再構成された行列はすべて有効制約からなる制約行列であり，これを行列 A とする．なお有効制約は暫定解が移動するたびに異なるため，暫定解の移動ごとに A を更新する必要がある．同様に \bf{\beta}(i) のみを抜き出してベクトル \bf{b} とする．これより A は，
\begin{aligned}A^T\bf{x} = \bf{b}\end{aligned}
の関係をもつ．
ここで線形制約がある場合の f(\bf{x}) に対する最適解 \bf{x}^* は，ラグランジュ乗数を \bf{\mu} (最適解の時 \bf{\mu}^* )としてKKT条件を用いると，
\begin{aligned}\left[\begin{array}{cc}G &-A\\-A &\bf{0}\end{array}\right] \left[\begin{array}{c}\bf{x}^*\\-\bf{\mu}^*\end{array}\right] = -\left[\begin{array}{c}\bf{g}\\-\bf{b}\end{array}\right]\end{aligned}
の関係を満たす．ここで，最適解 \bf{x}^* は G ， A から求まる行列 H ， D ， U を用いて，
\begin{aligned}\left[\begin{array}{c}\bf{x}^*\\-\bf{\mu}^*\end{array}\right] &=\left[\begin{array}{cc}-H &D\\D^T & -U\end{array}\right] \left[\begin{array}{c}\bf{g}\\\bf{b}\end{array}\right]\end{aligned}
として求まることが知られている(Fletcher (1987),元論文より)．ただし最適解に対する H らを求めるには，予め最適解におけるアクティブな制約がどれかが判明している必要があり，卵が先か鶏が先かのような問題になり現実的ではない．そのため最適解に対する H らを直接求めるのではなく，暫定解の移動による更新とそれに対する有効制約の更新を通して，逐次的に H らを計算していく．
1つ以上の違反する制約がある場合は暫定解を移動して更新する．ここで何らかの方法で最も大きく逸脱している制約 \bf{a}_{\mathrm{KNEXT}} を選び，この制約の境界線上まで暫定解 \bf{x} を移動させる．この時移動する方向(探索方向，search direction)は，ベクトル \bf{z} で表され，
\begin{aligned}\bf{z} = H\bf{a}_{\mathrm{KNEXT}}\end{aligned}
とする．この \bf{z} の方向に暫定解を移動する． k 回目の暫定解 \bf{x}_k を移動して k+1 回目の暫定解 \bf{x}_{k+1} に更新する．適切なスカラ t を用いると，
\begin{aligned}\bf{x}_{k+1} &=\bf{x}_{k} + t\bf{z}\\\mathrm{s.t.}\quad\bf{a}^T_{\mathrm{KNEXT}}\bf{x}_{k+1} &=\bf{b}(\mathrm{KNEXT})\end{aligned}
である． \bf{x}_{k+1} の違反する制約が無ければこれを最適解 \bf{x}^*=\bf{x}_{k+1} として求解を終了する．まだ違反する制約があるようならば \bf{a}_{\mathrm{KNEXT}} を新たに選び暫定解の移動を繰り返す．
これまでのQPKWIK法のアルゴリズムにおける H ， D ， U の計算をより詳しく見る． G は適切な下三角行列 L を用いて， G=LL^T に分解することが可能である．なお L はプログラム実行中にモデルが不変あるいは変化が予測できるものに限られるならば，実行前にすべて求めておくことができる．これより，行列積 G^{-1}A は，
\begin{aligned}G^{-1}A = L^{-T}L^{-1}A\end{aligned}
と展開され， L^{-1}A にQR分解を行い直交行列 Q と上三角行列 R に分解すると，
\begin{aligned}L^{-T}L^{-1}A = L^{-T}Q\left[\begin{array}{c}R\\0\end{array}\right]\end{aligned}
と表記できる．ここで Q を前半の列ベクトルの組と後半の列ベクトルの組の2つに分解する．分割の境界となるのは今引っかかっている制約の個数，すなわち有効制約の個数で，有効制約の個数だけ前半の列ベクトルをまとめた組を Q_1 ，それ以降の列ベクトルの組を Q_2 とする．行列 T を新たに用いると，
\begin{aligned}L^{-T}Q\left[\begin{array}{c}R\\0\end{array}\right] &= L^{-T}\Bigl[Q_1\;|\;Q_2\Bigr]\left[\begin{array}{c}R\\0\end{array}\right]\\&= [T_1\;|\;T_2\Bigr]\left[\begin{array}{c}R\\0\end{array}\right]\\&= T\left[\begin{array}{c}R\\0\end{array}\right]\\\end{aligned}
が求まる．以上までで求まった行列を用いて， H ， D ， U は，
\begin{aligned}H &= T_2T_2^T\\D &= T_1R^{-T}\\U &= -R^{-1}R^{-T}\end{aligned}
と計算される．MATLABのQPKWIK法をCコーダーによりCコード化したプログラムにおいては，行列積 L^{-1}A 以降の H ， D の計算を関数KWIKfactor()が担う．したがって関数KWIKfactor()は，
- 4回の行列積 L^{-1}A ， L^{-T}Q_1 ， T_2T_2^T ， T_1R^{-T}
- 1回のQR分解 \mathrm{QR}(L^{-1}A)
- 1回の三角行列の逆行列の計算 R^{-1}
の行列演算を行う． T_1 および T_2 の定義から，行列積 T_2T_2^T は制約が少ないほど計算量が大きくなるが，これを除く行列演算は制約が多いほど計算量が大きくなる．