非線形モデル予測制御

組み込み機器向けプロセッサの高速化に伴いモデル予測制御(Model Predictive Control; MPC)を機械系のリアルタイム制御に適用する事例が見られるようになってきました。 MPCの最も簡単な問題設定は線形システムを非拘束で制御するものです。 しかしながら、この問題設定では(終端コストに特殊な条件を加えるとですが)最適制御LQRと何も変わらない結果になります[Mayne et al., 2000]。 入力制約を加えても同様です。そうなるとPID制御で良いや…まで隣合わせで、いいところを全く見せられません。

そこで非線形システムの制約付き問題にMPCを適用することを考えます。

台車型倒立振子のふり上げ

非線形制御の検証に有名な台車型倒立振子のふり上げ問題(Cart-pole swing up)を取り扱います。振り子が取り付けられた左右に移動可能な台車を左右に振って振り子を逆立ち状態に持っていく問題です。

座標の定義等

制御は台車の左右に力を加えるとして入力制限を $ \pm 10 \text{N} $ 、台車の移動量に制限を加えて制約条件とします。

[参考]Cart Pole · Chachay/ClassicGym Wiki

状態量	単位	最小値	最大値
台車位置	m	-3	3
振り子角度	rad	$ -2 \pi $	$ 2 \pi $
台車位置	m/s	-10	10
振り子角度	rad/s	-10	10

非線形最適化問題

さて、非線形MPCでも、周期ごとに最適化問題を解くという点は変わりません。台車型倒立振子の逆立ち静止状態を目標の状態 $ x_{r} $ として二次計画法問題を立てます。

\[ \begin{array}{rl} \displaystyle \min_{\hat{x}, \hat{u}} & \sum_{i=0}^{N-1}(\frac{1}{2}(\hat{x}_{k} - x_{r})^{\intercal} Q (\hat{x}_{k} - x_{r}) + \frac{1}{2}\hat{u}_{k}^{\intercal} R \hat{u}_{k}) + \frac{1}{2}(\hat{x}_{N} - x_r)^{\intercal} Q_T (\hat{x}_{N} - x_r) \\ {\rm s.t.} & \hat{x}_0 = x(0) \\ & \underline{x} \preceq \hat{x}_t \preceq \bar{x} \\ & \underline{u} \preceq \hat{u}_t \preceq \bar{u} \\ & \hat{x}_{t+1} = f(\hat{x}_t, \hat{u}_t) \\ & t \in \{0, 1, \dots, N \} \end{array} \]

この問題は台車倒立振子のシステム $ x_{t+1} = f(x_{t}, u_t) $ が非線形の制約付き非線形最適化問題であります。この問題の解を今回は逐次二次計画法(Sequential Qadratic Problem; SQP)を使って求めます。

蛇足ですが、このとき、制御周期毎の制御出力は $ \hat{u}_0 $ です。

SQPでは、解候補を与えた上でSub Problemの二次計画法(Quadraic Problem; QP)を逐次解いて求めます。イメージとしてはニュートン法に似ています。 Sub Problemは原理的には前掲のWikipedia通りラグランジアンの未定乗数を導入してやるのですが、 ヘッシアンの大胆な近似(Gauss-Newton Hessian Approximation)[Bock, 1983]を行う実装上のノウハウを組み入れ、近似解 $ \hat{x}_{k}, \hat{u}_{k} $ (初期解)に対して

\[ \begin{array}{rl} \displaystyle \min_{x,u} & \sum_{i=0}^{N-1}\frac{1}{2}(x_{k} - x_r)^{\intercal} Q (x_{k} - x_r) + \frac{1}{2}u_{k}^{\intercal} R u_{k} + (\hat{x}_{k} - x_r)^{\intercal} Q x_{k} + \hat{u}_k R u_{k} \\ & + \frac{1}{2}(x_{N} - x_r)^{\intercal} Q_T (x_{N} - x_r) + (\hat{x}_{N} - x_r)^{\intercal} Q_T x_{N}\\ {\rm s.t.} & x_0 = x(0) \\ & \underline{x} \preceq x_t \preceq \bar{x} \\ & \underline{u} \preceq u_t \preceq \bar{u} \\ & x_{t+1} = A_k x_t + B_k u_t + g_k \\ & t \in \{0, 1, \dots, N \} \end{array} \]

(ただし $ A_k, B_k, g_k $ は、$ f(x, u) $ を $ \hat{x}_k, \hat{u}_k $ 近傍で線形化した係数 )

を解くような実装を見かけます。このSub Problemの解 $ x, u $ に対して $ \hat{x} \leftarrow \alpha \hat{x} + (1 - \alpha) x, \alpha \in [0, 1] $ として解を更新します。

もう少し補足

目的関数 $ g(x) = \frac{1}{2} x^\intercal Q x $ に対して大胆に $ \nabla_{xx}^{2} \mathcal{L}(x, \lambda, \sigma) \approx \nabla_{xx}^{2} g(x) $ としてしまいましょう。すると、

\[ \begin{array}{rl} \nabla_{xx}^{2}{\mathcal {L}} &\approx (\nabla x)^\intercal Q \nabla x + x^\intercal Q \nabla^{2} x \\ & = Q \end{array} \]

推定解 $ \hat{x} $ と微小変化 $ \delta x $ に対して $ x = \hat{x} + \delta x $ とすると

\[ \begin{array}{rl} g(\hat{x}) + \frac{1}{2} (\delta x)^\intercal \nabla_{xx}^{2} \mathcal{L} \delta x &\approx \frac{1}{2} \hat{x}^\intercal Q \hat{x} + \frac{1}{2} (\delta x)^\intercal Q \delta x \\ & = \frac{1}{2} x^\intercal Q x \end{array} \]

実装例

Gistに実装例を公開しました(An example of cart pole swing up by SQP.)

最適化問題のモデリングにcvxpyを使っているのでかなり遅いですが、コードの見通しは良いと思います。