付録A.4の補足稿を追加(part1)

suppl/.gitignore

@@ -0,0 +1,6 @@
+*.aux
+*.dvi
+*.log
+*.bbl
+*.blg
+

suppl/ocp_structured_qp.tex

@@ -0,0 +1,298 @@
+\documentclass[a4paper]{jarticle}
+\usepackage{amsmath,amssymb}
+
+\title{最適制御問題に固有の構造を利用した QP 問題の最適化計算（付録A.4の補足稿）}
+%\author{}
+%\date{}
+\begin{document}
+\maketitle
+
+線形MPCの場合や、非線形MPCで逐次二次計画法を用いる場合にはQP問題を扱います。
+このQP問題には最適制御に固有の構造があり、この固有の構造を利用して最適化計算をさらに高速化することが可能です。
+例えば、hpipm \cite{frison2020hpipm}は、この固有の構造を利用した内点法ベースのQPソルバーです。
+
+
+本補足稿では、QP 問題の内点法ベースの最適化計算において最適制御に固有の構造を利用する方法の概要を紹介します。
+なお、有効制約法や交互方向乗数法においても最適制御に固有の構造を利用することは可能であり、興味のある読者は \cite{nielsen2017low,sokoler2014input} などの手法を参照してください。
+
+\section*{準備}
+本補足稿で参照する目的で、本書\cite{fukatsu2024python}中で説明した式を以下に再掲します。
+
+\paragraph{本書\cite{fukatsu2024python}式(6.15)：線形MPCにおいて扱う離散時間最適制御問題の定式}
+
+\begin{equation}
+\begin{aligned}
+& \underset{ {\mathbf{X}},{\mathbf{U}}}{\text{minimize}} && 
+\mathbf{x}_K^TQ_K\mathbf{x}_K + 2 q_K^T\mathbf{x}_K
+\\
+&&&
++
+\sum_{k=0}^{K-1}
+\left\{
+\begin{bmatrix} \mathbf{x}_k \\ \mathbf{u}_k\end{bmatrix}^T
+\begin{bmatrix} Q_k & S_k \\ S_k^T & R_k \end{bmatrix}
+\begin{bmatrix} \mathbf{x}_k \\ \mathbf{u}_k\end{bmatrix}
++
+2\begin{bmatrix} q_k \\ r_k\end{bmatrix}^T
+\begin{bmatrix} \mathbf{x}_k \\ \mathbf{u}_k\end{bmatrix}
+\right\}
+\\
+&\text{subject to} && \left \{
+\begin{aligned}
+    & \mathbf{x}_0 = \bar{\mathbf{x}}_{init}\\
+    & \mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + b_k\\
+    & G^{x}_k \mathbf{x}_k + G^{u}_k\mathbf{u}_k \le h_k\\
+    & G^{x}_K\mathbf{x}_K\le h_K
+\end{aligned}
+\right .
+\end{aligned}
+\label{eq:ocp_qp}
+\end{equation}
+
+\paragraph{本書\cite{fukatsu2024python}式(A.9)：QP問題の内点法において扱うKKT行列線型方程式}
+
+\begin{align}
+\begin{bmatrix}
+Q & - C^T & O 
+\\
+- C & O & I 
+\\
+O & Z^\ell & \Pi^\ell
+\end{bmatrix}
+\begin{bmatrix}
+\Delta x
+\\
+\Delta \pi
+\\
+\Delta z
+\end{bmatrix}
+=
+\begin{bmatrix}
+-\gamma_x^\ell
+\\
+-\gamma_\pi^\ell
+\\
+-Z^\ell\Pi^\ell e +\mu^\ell e
+\end{bmatrix}
+\label{eq:ipm_qp}
+\end{align}
+
+\section{最適制御における不等式制約付き QP 問題の KKT 行列線形方程式計算の変換}
+
+不等式制約がある線形最適制御問題はQP問題として表現できます（本書\cite{fukatsu2024python}6.3.2項）。
+これを内点法で解く際に必要なKKT行列の線形方程式(\ref{eq:ipm_qp})を解く部分は、LQ制御問題に変換することが可能\cite{rao1998application}であり、LQ制御の効率的解法を使用して高速に解くことができます（後に本補足稿\ref{sec:riccati_recursion}節で説明します）。
+本節では、そのように高速に解くための前段階として、KKT行列の線形方程式(\ref{eq:ipm_qp})をLQ制御問題に変換することを数式を追いながら確認します。
+
+
+式(\ref{eq:ocp_qp})のQP問題における不等式制約をスラック変数$z_k$を用いて表現すると、次式となります。
+\begin{equation*}
+\begin{aligned}
+& \underset{ \mathbf{X},\mathbf{U}}{\text{min}} && 
+\mathbf{x}_K^TQ_K\mathbf{x}_K + 2 q_K^T\mathbf{x}_K
++
+\sum_{k=0}^{K-1}
+\begin{bmatrix} \mathbf{x}_k \\ \mathbf{u}_k \\ 1\end{bmatrix}^T
+\begin{bmatrix} Q_k & S_k & q_k \\ S_k^T & R_k & r_k \\ q_k^T & r_k^T & 0\end{bmatrix}
+\begin{bmatrix} \mathbf{x}_k \\ \mathbf{u}_k \\ 1\end{bmatrix} \\
+&\text{subject to} && \left \{
+\begin{aligned}
+    & \mathbf{x}_0 = \bar{\mathbf{x}}_{init}\\
+    & \mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + b_k\\
+    & G^{x}_k \mathbf{x}_k + G^{u}_k\mathbf{u}_k + z_k =h_k\\
+    & G^{x}_K\mathbf{x}_K + z_K = h_K\\
+    & z_k \ge 0
+\end{aligned}
+\right .
+\end{aligned}
+\end{equation*}
+このQP問題の解が満たすべき最適性の必要条件は、次のKKT条件です。
+\begin{align*}
+\mathbf{x}_0 &= \bar{\mathbf{x}}_{init}
+\\
+Q_k \mathbf{x}_k + S_k \mathbf{u}_k + A_k^T\lambda_k - \lambda_{k-1} + (G_k^x)^T\pi_k &= -q_k
+\\
+S_k^T\mathbf{x}_k + R_k \mathbf{u}_k + B_k^T\lambda_k + (G_k^u)^T\pi_k &= - r_k
+\\
+A_k \mathbf{x}_k + B_k \mathbf{u}_k  - \mathbf{x}_{k+1} &= - b_k
+\\
+G_k^x\mathbf{x}_k + {G_k^u}\mathbf{u}_k + z_k &= h_k
+\\
+G_K^x\mathbf{x}_k+ z_K &= h_K
+\\
+Z_k\Pi_ke &=0
+\\
+\pi_k &\ge0\\
+z_k &\ge 0
+\end{align*}
+ここで、
+初期状態等式制約の未定乗数を$\lambda_{-1}$、
+$k$ステップ目の状態方程式モデルの等式制約に対する未定乗数を$\lambda_{k}$、
+$k$ステップ目の不等式制約のスラック変数を用いた等式表現に対する未定乗数を$\pi_k$としています。
+本書\cite{fukatsu2024python}付録A.2.1で述べたように、内点法ではこのKKT条件を現在の推定解周りで線形化して修正し、得られたKKT行列の線形方程式を解くことにより更新方向を計算します。
+
+
+不等式制約には$k=0,\cdots,K-1$ステップ目の不等式制約$G_k^x\mathbf{x}_k + G_k^u\mathbf{u}_k<h_k$と$k=K$ステップ目の不等式制約$G_K^x\mathbf{x}_K<h_K$の2種類が存在します。
+まず、$k=0,\cdots,K-1$ステップ目の不等式制約を扱う方法を説明します。
+KKT行列の線形方程式の中で$k$ステップ目の変数$(\mathbf{x}_k,\mathbf{u}_k,\pi_k,z_k,\lambda_k)$に関する勾配についての最適性の必要条件に対応する行は、関係する成分のみを取り出して表記すると次のように書くことができます。
+\begin{align}
+\begin{bmatrix}
+ -I &  Q_{k} & S_{k} & (G_{k}^x)^T & O & A_{k}^T & O \\
+ O &  S_{k}^T & R_{k} & (G_{k}^u)^T & O & B_{k}^T & O \\
+ O  & G_{k}^x      & G_{k}^u & O  & I &  O & O\\
+O  & O       & O       & Z_{k} & \Pi_{k}   &  O & O\\
+O  & A_{k} & B_{k} & O & O   &  O & -I
+\end{bmatrix}
+\begin{bmatrix}
+\Delta \lambda_{k-1}\\
+\Delta \mathbf{x}_{k}\\
+\Delta \mathbf{u}_{k}\\
+\Delta \pi_{k}\\
+\Delta z_{k}\\
+\Delta \lambda_{k}\\
+\Delta \mathbf{x}_{k+1}
+\end{bmatrix}
+=
+\begin{bmatrix}
+-\gamma_{x_k}\\
+-\gamma_{u_k}\\
+-\gamma_{\pi_k}\\
+-\gamma_{z_k}\\
+-\gamma_{\lambda_k}\\
+\end{bmatrix}
+\label{eq:before_remove_path_inequality}
+\end{align}
+ここで、式(\ref{eq:before_remove_path_inequality})の右辺は現在の推定解の残差に内点法による修正を加えたものであり、式(\ref{eq:ipm_qp})の右辺と同様です。
+式(\ref{eq:before_remove_path_inequality})の3行目と4行目より、$\Delta \pi_k$と$\Delta z_k$は次式により得られます。
+\begin{align*}
+\Delta z_k &= -G_K^x\Delta x_k-G_k^u\Delta u_k-\gamma_{\pi_k}
+\\
+\Delta \pi_k &= Z_k^{-1}(-\Pi_k\Delta z_k -\gamma_{z_k})
+\\
+&= Z_k^{-1}\Pi_k(G_k^x\Delta x_k + G_k^u\Delta u_k)
++Z_k^{-1}(\Pi_k\gamma_{\pi_k}-\gamma_{z_k})
+\end{align*}
+これらを式(\ref{eq:before_remove_path_inequality})の1行目に代入すると、次式が得られます。
+\begin{align}
+\begin{bmatrix}
+ -I &  Q_{k}' & S_{k}' & A_{k}^T & O \\
+ O &  S_{k}'^T & R_{k}' & B_{k}^T & O \\
+O  & A_{k} & B_{k} &  O & -I
+\end{bmatrix}
+\begin{bmatrix}
+\Delta \lambda_{k-1}\\
+\Delta \mathbf{x}_{k}\\
+\Delta \mathbf{u}_{k}\\
+\Delta \lambda_{k}\\
+\Delta \mathbf{x}_{k+1}
+\end{bmatrix}
+=
+\begin{bmatrix}
+-\gamma_{x_{k}}'\\
+-\gamma_{u_{k}}'\\
+-\gamma_{\lambda_{k}}\\
+\end{bmatrix}
+\label{eq:after_remove_path_inequality}
+\end{align}
+ここで、以下のように省略表記しました。
+\begin{align*}
+Q_{k}' &=Q_k +(G_k^x)^TZ_k^{-1}\Pi_kG_k^x
+\\
+S_{k}' &=S_k +(G_k^x)^TZ_k^{-1}\Pi_kG_k^u
+\\
+R_{k}' &=R_k +(G_k^u)^TZ_k^{-1}\Pi_kG_k^u
+\\
+\gamma_{x_k}' &=\gamma_{x_k}+(G_k^x)^TZ_k^{-1}(\Pi_k\gamma_{\pi_k}-\gamma_{z_k})
+\\
+\gamma_{u_k}' &=\gamma_{u_k}+(G_k^u)^TZ_k^{-1}(\Pi_k\gamma_{\pi_k}-\gamma_{z_k})
+\end{align*}
+
+
+次に、$k=K$ステップ目の終端状態の不等式制約を扱う方法を説明します。
+変数$\mathbf{x}_K,\pi_K,z_K$に関する勾配の最適性の必要条件に対応する行を式(\ref{eq:before_remove_path_inequality})と同様に表記すると、次式となります。
+\begin{align}
+\begin{bmatrix}
+ -I & Q_{K} & (G_K^x)^T    & O \\
+ O & G_K^x &  O& I\\
+ O & O & Z_K & \Pi_K
+\end{bmatrix}
+\begin{bmatrix}
+\Delta \lambda_{K-1}\\
+\Delta \mathbf{x}_K\\
+\Delta \pi_K\\
+\Delta z_K
+\end{bmatrix}
+=
+\begin{bmatrix}
+-\gamma_{x_K}\\
+-\gamma_{\pi_K}\\
+-\gamma_{z_K}
+\end{bmatrix}
+\label{eq:remove_terminal_inequality}
+\end{align}
+式(\ref{eq:remove_terminal_inequality})の2行目と3行目より、$\Delta \pi_K$と$\Delta z_K$はそれぞれ次式となります。
+\begin{align*}
+\Delta z_K &= -G_K^x\Delta x_K-\gamma_{\pi_K}
+\\
+\Delta \pi_K &= Z_K^{-1}(-\Pi_K\Delta z_K -\gamma_{z_K})
+= Z_K^{-1}\Pi_KG_K^x\Delta x_K
++Z_K^{-1}(\Pi_K\gamma_{\pi_K} - \gamma_{z_K})
+\end{align*}
+これらを式(\ref{eq:remove_terminal_inequality})の1行目に代入すると、次式が得られます。
+\begin{align}
+\begin{bmatrix}
+-I & Q_K'
+\end{bmatrix}
+\begin{bmatrix}
+\Delta \lambda_{K-1}\\
+\Delta \mathbf{x}_K\\
+\end{bmatrix}
+=
+-\gamma_{x_K}'
+\label{eq:after_remove_terminal_inequality}
+\end{align}
+ここで、
+\begin{align*}
+ Q_K' &= Q_K +(G_K^x)^TZ_{K}^{-1}\Pi_KG_K^x
+ \\
+ \gamma_{x_K}' &= \gamma_{x_K} + (G_K^x)^TZ_K^{-1}(\Pi_K\gamma_{\pi_K}-\gamma_{z_K})
+\end{align*}
+そのため、
+最適制御において不等式制約条件を含むQP問題を解く際の内点法の中でKKT行列の線形方程式を解いて更新方向を計算する部分は、
+式(\ref{eq:after_remove_path_inequality})と式(\ref{eq:after_remove_terminal_inequality})を満たす$(\Delta x_k,\Delta u_k,\Delta \lambda_k)$を計算することと等価です。
+
+
+式(\ref{eq:after_remove_path_inequality})と式(\ref{eq:after_remove_terminal_inequality})は、
+次のQP問題の解$(\Delta x_k,\Delta u_k,\Delta \lambda_k)$が満たすべき最適性の必要条件として見ることもできます。
+\begin{equation*}
+\begin{aligned}
+& \underset{ \Delta\mathbf{X}, \Delta\mathbf{U}}{\text{min}} && 
+\Delta \mathbf{x}_K^TQ_K'\Delta \mathbf{x}_K + 2 \gamma_{x_K}'^T\Delta \mathbf{x}_K
+\\
+&  && 
++
+\sum_{k=0}^{K-1}
+\begin{bmatrix} \Delta \mathbf{x}_k \\ \Delta \mathbf{u}_k \\ 1\end{bmatrix}^T
+\begin{bmatrix} Q_k' & S_k' & \gamma_{x_k}' \\ S_k'^T & R_k' & \gamma_{u_k}' \\ \gamma_{x_k}'^T & \gamma_{u_k}'^T & 0\end{bmatrix}
+\begin{bmatrix} \Delta \mathbf{x}_k \\ \Delta \mathbf{u}_k \\ 1\end{bmatrix} \\
+&\text{subject to} && \left \{
+\begin{aligned}
+    & \Delta\mathbf{x}_0 = \bar{\mathbf{x}}_{init} - \tilde{\mathbf{x}}_{0}\\
+    & \Delta \mathbf{x}_{k+1} = A_k \Delta \mathbf{x}_k + B_k \Delta \mathbf{u}_k + \gamma_{\lambda_k}\\
+\end{aligned}
+\right .
+\end{aligned}
+\end{equation*}
+ここで、$\tilde{\mathbf{x}}_{0}$は現在の$\mathbf{x}_0$の推定解です。
+このQP問題は、評価関数に交差項があり状態方程式にバイアス項がある場合の有限ホライズンの離散時間LQ制御問題として見ることができます。
+
+以上により、最適制御において不等式制約条件を含むQP問題を解く際の内点法の中のKKT行列の線形方程式を解いて更新方向を計算する部分は、
+有限ホライズンの離散時間LQ制御問題の解を計算することに変換することができることを確認しました。
+
+\section{リッカチ再帰式を利用したKKT行列の線形方程式の解法}
+\label{sec:riccati_recursion}
+\section{partial condensing（状態変数の部分的な消去）}
+
+\bibliographystyle{unsrt}
+\bibliography{ref}
+
+\end{document}

suppl/ref.bib

@@ -0,0 +1,58 @@
+@phdthesis{frison2015algorithms,
+  author  = "Gianluca Frison",
+  title   = "Algorithms and Methods for High-Performance Model Predictive Control",
+  school  = "Technical University of Denmark",
+  year    = "2015"
+}
+
+@article{frison2020hpipm,
+  title={HPIPM: a high-performance quadratic programming framework for model predictive control},
+  author={Frison, Gianluca and Diehl, Moritz},
+  journal={IFAC-PapersOnLine},
+  volume={53},
+  number={2},
+  pages={6563--6569},
+  year={2020},
+  publisher={Elsevier}
+}
+
+@article{nielsen2017low,
+  title={Low-rank modifications of Riccati factorizations for model predictive control},
+  author={Nielsen, Isak and Axehill, Daniel},
+  journal={IEEE Transactions on Automatic Control},
+  volume={63},
+  number={3},
+  pages={872--879},
+  year={2017},
+  publisher={IEEE}
+}
+
+@article{rao1998application,
+  title={Application of interior-point methods to model predictive control},
+  author={Rao, CV and Wright, SJ and Rawlings, JB},
+  journal={Journal of optimization theory and applications},
+  volume={99},
+  pages={723--757},
+  year={1998},
+  publisher={Springer}
+}
+
+@inproceedings{sokoler2014input,
+  title={Input-constrained model predictive control via the alternating direction method of multipliers},
+  author={Sokoler, Leo Emil and Frison, Gianluca and Andersen, Martin S and J{\o}rgensen, John Bagterp},
+  booktitle={2014 European Control Conference (ECC)},
+  pages={115--120},
+  year={2014},
+  organization={IEEE}
+}
+
+@book{fukatsu2024python,
+  title={PythonとCasADiで学ぶモデル予測制御},
+  author={深津卓弥 and 菱沼徹 and 荒牧大輔},
+  isbn={9784065356111},
+  series={KS理工学専門書},
+  url={https://books.google.co.jp/books?id=PJy00AEACAAJ},
+  year={2024},
+  publisher={講談社}
+}
+