Skip to content

Instantly share code, notes, and snippets.

@ekusiadadus

ekusiadadus/introduction_to_numerical_optimal_control.ipynb

Forked from mayataka/introduction_to_numerical_optimal_control.ipynb
Created May 25, 2022 13:16
Show Gist options
  • Save ekusiadadus/0c85f47c239a5205eb3614fef778205b to your computer and use it in GitHub Desktop.
Save ekusiadadus/0c85f47c239a5205eb3614fef778205b to your computer and use it in GitHub Desktop.
Download ZIP
数値最適制御入門の入門
Display the source blob
Display the rendered blob
Raw
introduction_to_numerical_optimal_control.ipynb
{
"cells": [
{
"cell_type": "markdown",
"id": "78ccb7e5",
"metadata": {},
"source": [
"# 数値最適制御入門の入門\n",
"Author: 京都大学 大学院情報学研究科 片山想太郎\n",
" "
]
},
{
"cell_type": "markdown",
"id": "2fe8631e",
"metadata": {},
"source": [
"本 notebookは，数値最適制御の入門テキストを読むための動機付けとなることを狙って書きました． \n",
"最適制御理論（特に連続時間最適制御）についての入門テキストとしては\n",
"- [大塚敏之, 非線形最適制御入門，コロナ社](https://www.coronasha.co.jp/np/isbn/9784339033182/)\n",
"\n",
"数値最適制御については\n",
"- [Sebastien Gros and Moritz M. Diehl, Numerical Optimial Control](https://www.syscop.de/files/2020ss/NOC/book-NOCSE.pdf)\n",
"- [James B. Rawlings, David Q. Mayne, and Moritz M. Dieh, Model Predictive Control: Theory, Computation, and Design, 2nd section, Nob Hill Publishing](https://sites.engineering.ucsb.edu/~jbraw/mpc/) (特に Chapter 8)\n",
"\n",
"数値最適化全般については\n",
"- [Jorge Nocedal, Stephen J. Wright, Numerical Optimization, Springer](http://www.apmath.spbu.ru/cnsa/pdf/monograf/Numerical_Optimization2006.pdf) \n",
"\n",
"を参考にしてください．"
]
},
{
"cell_type": "markdown",
"id": "86a8b193",
"metadata": {},
"source": [
"## 1. 問題設定\n",
"\n",
"### 制御対象システム\n",
"<img src=\"https://raw.github.com/wiki/mayataka/CGMRES/images/fig_cartpole.png\" width=30%>\n",
"\n",
"Cartpoleと呼ばれるこちらの非線形システムの振り上げ制御を考えます．ラグランジュの運動方程式から加速度の式が以下のように得られます．\n",
"$$\\ddot{y} = \\frac{1}{m_c + m_p \\sin ^2{\\theta}} \\left\\{ u + m_p \\sin{\\theta} (l \\dot{\\theta}^2 + g \\cos{\\theta}) \\right\\}, $$ \n",
"$$\\ddot{\\theta} = \\frac{1}{l(m_c + m_p \\sin ^2{\\theta})} \\left\\{ - u \\cos{\\theta} - m_p l {\\dot{\\theta}}^2 \\cos{\\theta} \\sin{\\theta} - (m_c + m_p) g \\sin{\\theta} \\right\\} .$$\n",
"\n",
"### 制約\n",
"制約として，制御入力（カートにかかる力）の上下限\n",
"$$u_{\\rm min} \\leq u \\leq u_{\\rm max} , $$\n",
"およびカートの位置についての制約\n",
"$$y_{\\rm min} \\leq y \\leq y_{\\rm max} $$\n",
"が課されているとします．\n",
"\n",
"### 制御目標\n",
"制御目標はカートの位置 $y=0$ 周辺でポールを倒立させること（$\\theta=\\pi$）です．"
]
},
{
"cell_type": "markdown",
"id": "7cda93bc",
"metadata": {},
"source": [
"## 2. 最適制御問題の定式化\n",
"### 状態方程式\n",
"（有限時間・連続時間の）最適制御問題を定式化しています．\n",
"状態ベクトルを\n",
"$$ x = \\begin{bmatrix} y \\\\ \\theta \\\\ \\dot{y} \\\\ \\dot{\\theta} \\end{bmatrix} $$\n",
"とすると，状態方程式は\n",
"$$\\dot{x} = f(x, u) = \\begin{bmatrix} \\dot{y} \\\\ \\dot{\\theta} \\\\ \\frac{1}{m_c + m_p \\sin ^2{\\theta}} \\left\\{ u + m_p \\sin{\\theta} (l \\dot{\\theta}^2 + g \\cos{\\theta}) \\right\\} \\\\ \\frac{1}{l(m_c + m_p \\sin ^2{\\theta})} \\left\\{ - u \\cos{\\theta} - m_p l {\\dot{\\theta}}^2 \\cos{\\theta} \\sin{\\theta} - (m_c + m_p) g \\sin{\\theta} \\right\\} \\end{bmatrix} .$$\n",
"と得られます．\n",
"### 制約\n",
"また制約をまとめて\n",
"$$\n",
"g(x, u) = \\begin{bmatrix} u_{\\rm min} - u \\\\ u - u_{\\rm max} \\\\ y_{\\rm min} - y \\\\ y - y_{\\rm max} \\end{bmatrix} \\leq 0\n",
"$$\n",
"と書きます．\n",
"この問題では現れていませんが，\n",
"$$\n",
"c(x, u) = 0\n",
"$$\n",
"のような等式制約が課されることもあります．\n",
"\n",
"### 評価関数\n",
"最適制御では，状態$x$がこの状態方程式と制約に従って時間発展していくときに，ある評価関数（目的関数）\n",
"$$ J = V_f (x(t_f)) + \\int_{t_0}^{t_f} l (x(t), u(t)) dt. $$\n",
"を最小化することが目的となります．\n",
"今回は状態を $$x_{\\rm ref} := \\begin{bmatrix} 0 \\\\ \\pi \\\\ 0 \\\\ 0 \\end{bmatrix}$$ に近づけることが目的です．また，制御入力は小さい方が望ましいです．\n",
"そこで，以下のような二次形式の終端コスト\n",
"$$V_f (x) = \\frac{1}{2} (x - x_{\\rm ref})^{\\rm T} Q_{f} (x - x_{\\rm ref}) ,$$ \n",
"およびステージコスト\n",
"$$l(x, u) = \\frac{1}{2} (x - x_{\\rm ref})^{\\rm T} Q (x - x_{\\rm ref}) + \\frac{1}{2} u^{\\rm T} R u $$ \n",
"を考えます．\n",
"ただし，$Q_f， Q, R$ は正定値対角行列です．\n",
"\n",
"### まとめ\n",
"最後に，最適制御問題をまとめます． 今求めたいものは最適な制御入力 $u (t)$, $t_0 \\leq t < t_f$です．また初期時刻$t_0$での状態は与えられているものとします．\n",
"$$\n",
"\\min_{u} {J} = V_f (x(t_f)) + \\int_{t_0}^{t_f} l (x(t), u(t)) dt \\\\\n",
"{\\rm s.t.} \n",
"\\;\\;\\; x(t_0) = \\bar{x}, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\;\\;\\;\\;\\;\\; \\dot{x} (t) = f(x(t), u(t)), \\;\\;\\; t_0 \\leq t < t_f \\\\\n",
"\\;\\;\\;\\;\\; g(x(t), u(t)) \\leq 0, \\;\\;\\; t_0 \\leq t < t_f \\\\\n",
"\\;\\;\\; g_f (x(t_f)) = 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\; c(x(t), u(t)) = 0, \\;\\;\\; t_0 \\leq t < t_f \\\\\n",
"\\;\\;\\; c_f (x(t_f)) = 0 \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"$$"
]
},
{
"cell_type": "markdown",
"id": "f9058541",
"metadata": {},
"source": [
"## 3. 数値最適制御問題の定式化\n",
"### 数値最適制御(MPC)のアプローチ\n",
"<img src=\"https://user-images.githubusercontent.com/33686357/170175375-d39714df-ea41-4438-ba63-01bea8094790.png\" width=60%>\n",
"\n",
"上記の問題は連続時間の問題で無限次元の最適化問題となっているため一般には解けません．\n",
"そこで，有限次元の最適化問題(非線形計画問題; NLP)に落とし込む， direct 法 (direct optimal control) というアプローチをとります．　 \n",
"一方で， Hamilton-Jacobi-Bellman 方程式を用いるものや，indirect 法と呼ばれるアプローチもあります．しかし複雑な問題に対しては direct 法が最も有効です． \n",
"\n",
"また，direct 法の中でも 状態と制御入力両方を決定変数として最適化する simultaneous 法と，制御入力についてだけの最適化問題に帰着させる sequenatial 法 (single shooting 法)があります．今回は，計算速度や収束性能の観点から非線形MPCに有効な simultaneous 法を選びます．\n",
"Simultaneous 法の中にも multiple shooting 法と collocation 法があり，今回はシンプルで計算時間の観点からMPCに有効な multiple shooting法を選びます． \n",
"(ちなみに， collocation 法 と　multiple shooting 法を混同している論文などもたまに見かけますが，似ているようで根本的に違うアイデアに基づいて定式化しています．注意です．) \n",
"有効なアプローチは問題や用途によって異なるため，色々なアプローチを知っておくことが大事だと思います． \n",
"\n",
"### Multiple shooting 法\n",
"\n",
"<img src=\"https://user-images.githubusercontent.com/33686357/170209287-6d2d21d9-f38e-43e4-8f1a-318c30c1f65b.png\" width=60%>\n",
"\n",
"上図のように， multiple shooting 法では$N$個の離散化グリッドを導入し，それぞれに制御入力や状態を割り当てます． \n",
"Multiple shooting 法の中でも連続時間の状態軌道の離散化方法によりさらにアプローチが異なります． \n",
"今回は最もシンプルな forward Euler 法を選びます．（もちろんこの選び方も問題設定や用途によって異なります．） \n",
"\n",
"\n",
"ホライゾン $[t_0, t_f]$　を$N$ ステップに離散化しましょう． このとき1離散化したグリッドごとの間隔を $\\Delta \\tau := (t_f - t_0) / N$とします．\n",
"Forward Euler 法では，$\\dot{x} = f(x, u)$は，時間ステップ$\\Delta \\tau$を用いて\n",
"$$x_{i+1} = x_i + f(x_i, u_i) \\Delta \\tau$$\n",
"と近似されます．Multiple-shootingでは，これを，各グリッドでの\"ギャップ\"についての等式制約\n",
"$$x_i + f(x_i, u_i) \\Delta \\tau - x_{i+1} = 0$$\n",
"として考慮します．\n",
"また評価関数は\n",
"$$\\int_{t_0}^{t_f} l (x(t), u(t)) dt = \\sum_{i=0}^{N-1} l(x_i, u_i) \\Delta \\tau $$ となります．\n",
"\n",
"### まとめ\n",
"最後に，数値最適制御問題をまとめます． 今求めたいものは最適な状態と制御入力の系列 $x_0, ..., x_N$, $u_0, ..., u_{N-1}$です．\n",
"$$\n",
"\\min_{x_0, ..., x_N, u_0, ..., u_{N-1}} {J} = V_f (x_N) + \\sum_{i=0}^{N-1} l(x_i, u_i) \\Delta \\tau \\\\\n",
"{\\rm s.t.} \\;\\; x_0 = \\bar{x}, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; x_i + f(x_i, u_i) \\Delta \\tau - x_{i+1} = 0, \\;\\;\\; i=0, ..., N-1 \\\\\n",
"\\;\\;\\; g (x_i, u_i) \\leq 0, \\;\\;\\; i=0, ..., N-1 \\\\\n",
"\\;\\;\\; g_f (x_N) \\leq 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\; c (x_i, u_i) = 0, \\;\\;\\; i=0, ..., N-1 \\\\\n",
"\\;\\;\\; c (x_N) = 0. \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"$$\n",
"これは非線形計画問題（NLP）と呼ばれるクラスの最適化問題であり，汎用NLPソルバーを使って解くことができます．"
]
},
{
"cell_type": "markdown",
"id": "d65eef29",
"metadata": {},
"source": [
"## 4. 数値最適制御ツール\n",
"ここでは CasADi というツールで数値最適制御問題を解きます． \n",
"まず以下のコマンドでインストールしましょう．\n",
"```\n",
"pip install casadi\n",
"```\n",
"実際にはCasADiは定式化を行うためのツールで，裏でIpoptなどの汎用的な数値最適化ソルバーを呼んでいます．早速インポートしましょう"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "2e4e6d4c",
"metadata": {},
"outputs": [],
"source": [
"from casadi import *"
]
},
{
"cell_type": "markdown",
"id": "5c765e2e",
"metadata": {},
"source": [
"### モデルと評価関数の設定\n",
"モデルと評価関数を記述します．記述方法はどのようにしてもよいですが，ここではクラスとして記述します．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e5dc290e",
"metadata": {},
"outputs": [],
"source": [
"class CartPole:\n",
" def __init__(self):\n",
" self.mc = 2.0 # cart の質量[kg]\n",
" self.mp = 0.2 # pole の質量[kg]\n",
" self.l = 0.5 # pole の長さ[m]\n",
" self.ga = 9.81 # 重力加速度[m/s2]\n",
"\n",
" def dynamics(self, x, u):\n",
" mc = self.mc\n",
" mp = self.mp\n",
" l = self.l\n",
" ga = self.ga\n",
" y = x[0] # cart の水平位置[m]\n",
" th = x[1] # pole の傾き角[rad]\n",
" dy = x[2] # cart の水平速度[m/s]\n",
" dth = x[3] # pole の傾き角速度[rad/s]\n",
" f = u[0] # cart を押す力[N]（制御入力）\n",
" # cart の水平加速度\n",
" ddy = (f+mp*sin(th)*(l*dth*dth+ga*cos(th))) / (mc+mp*sin(th)*sin(th)) \n",
" # pole の傾き角加速度\n",
" ddth = (-f*cos(th)-mp*l*dth*dth*cos(th)*sin(th)-(mc+mp)*ga*sin(th)) / (l * (mc+mp*sin(th)*sin(th))) \n",
" return dy, dth, ddy, ddth"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "6752c653",
"metadata": {},
"outputs": [],
"source": [
"# コスト関数を記述\n",
"class CostFunction:\n",
" def __init__(self):\n",
" self.nx = 4\n",
" self.nu = 1\n",
" self.x_ref = [0.0, pi, 0.0, 0.0] # 目標状態\n",
" # ステージコストのパラメータ \n",
" self.Q = [2.5, 10.0, 0.01, 0.01] # 状態への重み\n",
" self.R = [0.1] # 制御入力への重み\n",
" # 終端コストのパラメータ \n",
" self.Qf = [2.5, 10.0, 0.01, 0.01] # 状態への重み\n",
"\n",
" # ステージコスト\n",
" def stage_cost(self, x, u):\n",
" l = 0 \n",
" for i in range(self.nx):\n",
" l += 0.5 * self.Q[i] * (x[i]-self.x_ref[i])**2 \n",
" for i in range(self.nu):\n",
" l += 0.5 * self.R[i] * u[i]**2\n",
" return l\n",
"\n",
" # 終端コスト\n",
" def terminal_cost(self, x):\n",
" Vf = 0\n",
" for i in range(self.nx):\n",
" Vf += 0.5 * self.Q[i] * (x[i]-self.x_ref[i])**2 \n",
" return Vf"
]
},
{
"cell_type": "markdown",
"id": "0c38a433",
"metadata": {},
"source": [
"最適制御の問題設定も定義します．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "a80499a5",
"metadata": {},
"outputs": [],
"source": [
"# 問題設定\n",
"T = 5.0 # ホライゾン長さ\n",
"N = 100 # ホライゾン離散化グリッド数\n",
"dt = T / N # 離散化ステップ\n",
"nx = 4 # 状態空間の次元\n",
"nu = 1 # 制御入力の次元"
]
},
{
"cell_type": "markdown",
"id": "83f9ac6b",
"metadata": {},
"source": [
"次に，CasADiで問題を定式化するために，変数や制約を格納するlistを作ります．\n",
"- 最適化変数変数\n",
" - w: 最適化変数（状態$x_0, ..., x_N$, 制御入力 $u_0, ..., u_{N-1}$）\n",
" - w0: 最適化変数の初期推定解（全て0でもOK）\n",
" - lbw, ubw: 最適化変数の下界と上界（なければ inf を設定）\n",
"\n",
"\n",
"- 制約\n",
" - g: 制約の式を格納\n",
" - lbg, ubg: 制約の式の上下界，すなわち $lbg \\leq g (x,u) \\leq ubg$. \n",
" - $lbg=ubg$と設定することで， 等式制約 $g (x,u) = lbg = ubg$ にできる． \n",
"\n",
"\n",
" - 評価関数 \n",
" - J: 評価関数を全てこの項に足し合わせていきます．"
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "598bddde",
"metadata": {},
"outputs": [],
"source": [
"# 以下で非線形計画問題(NLP)を定式化\n",
"w = [] # 最適化変数を格納する list\n",
"w0 = [] # 最適化変数(w)の初期推定解を格納する list\n",
"lbw = [] # 最適化変数(w)の lower bound を格納する list\n",
"ubw = [] # 最適化変数(w)の upper bound を格納する list\n",
"g = [] # 制約（等式制約，不等式制約どちらも）を格納する list\n",
"lbg = [] # 制約関数(g)の lower bound を格納する list\n",
"ubg = [] # 制約関数(g)の upper bound を格納する list\n",
"J = 0 # 評価関数 "
]
},
{
"cell_type": "markdown",
"id": "db8ae3ae",
"metadata": {},
"source": [
"for 文を使ってステージコスト，状態方程式，その他制約を逐次的に記述していきます． \n",
"まずは初期状態 $x_0$ の値は $[0, 0, 0, 0]$ に固定するため，これを $w$の上下限として設定します．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "45d7948a",
"metadata": {},
"outputs": [],
"source": [
"Xk = MX.sym('X0', nx) # 初期時刻の状態ベクトル x0\n",
"w += [Xk] # x0 を 最適化変数 list (w) に追加\n",
"# 初期状態は given という条件を等式制約として考慮\n",
"lbw += [0., 0., 0., 0.] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
"ubw += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
"w0 += [0, 0, 0, 0] # x0 の初期推定解"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "c1e3f5a7",
"metadata": {},
"outputs": [],
"source": [
"cartpole = CartPole()\n",
"cost = CostFunction()"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "163b0463",
"metadata": {},
"outputs": [],
"source": [
"# 離散化ステージ k=0~N-1 までのステージコストと制約を設定\n",
"for k in range(N):\n",
" Uk = MX.sym('U_' + str(k), nu) # 時間ステージ k の制御入力 uk を表す変数\n",
" w += [Uk] # uk を最適化変数 list に追加\n",
" lbw += [-5.0] # uk の lower-bound\n",
" ubw += [5.0] # uk の upper-bound\n",
" w0 += [0] # uk の初期推定解\n",
"\n",
" # コスト関数にステージコスト l(x_k, u_k) を追加\n",
" J = J + cost.stage_cost(Xk, Uk) * dt \n",
"\n",
" # Forward Euler による離散化状態方程式\n",
" dXk = cartpole.dynamics(Xk, Uk) # xdot＝f(x, u)\n",
" Xk_next = vertcat(Xk[0] + dXk[0] * dt, \n",
" Xk[1] + dXk[1] * dt,\n",
" Xk[2] + dXk[2] * dt,\n",
" Xk[3] + dXk[3] * dt)\n",
" Xk1 = MX.sym('X_' + str(k+1), nx) # 時間ステージ k+1 の状態 xk+1 を表す変数\n",
" w += [Xk1] # xk+1 を最適化変数 list に追加\n",
" lbw += [-10., -inf, -inf, -inf] # xk+1 の lower-bound （指定しない要素は -inf）\n",
" ubw += [10., inf, inf, inf] # xk+1 の upper-bound （指定しない要素は inf）\n",
" w0 += [0.0, 0.0, 0.0, 0.0] # xk+1 の初期推定解\n",
"\n",
" # 状態方程式(xk+1=xk+fk*dt) を等式制約として導入\n",
" g += [Xk_next-Xk1]\n",
" lbg += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" ubg += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" Xk = Xk1"
]
},
{
"cell_type": "markdown",
"id": "c5bfd3c2",
"metadata": {},
"source": [
"最後に終端コストを追加して数値最適制御問題の記述完了です．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e3a56656",
"metadata": {},
"outputs": [],
"source": [
"J = J + cost.terminal_cost(Xk)"
]
},
{
"cell_type": "markdown",
"id": "db6f3f1a",
"metadata": {},
"source": [
"内点法ソルバーIpoptを用いてこちらの最適制御問題（非線形計画問題，NLP）を解きます． \n",
"nlp という値に評価関数（J），最適化変数(x)，制約（g）を格納します． \n",
"そして，nlpを用いて nlpsol というソルバークラスを呼び出します．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "51b2c2d8",
"metadata": {},
"outputs": [],
"source": [
"# 非線形計画問題(NLP)\n",
"nlp = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} \n",
"# Ipopt ソルバー，最小バリアパラメータを0.001に設定\n",
"solver = nlpsol('solver', 'ipopt', nlp, {'ipopt':{'mu_min':0.001}}) \n",
"# SQP ソルバー（QPソルバーはqpOASESを使用），QPソルバーの regularization 無効，QPソルバーのプリント無効\n",
"# solver = nlpsol('solver', 'sqpmethod', nlp, {'max_iter':100, 'qpsol_options':{'enableRegularisation':False, 'printLevel':None}})"
]
},
{
"cell_type": "markdown",
"id": "300a40c1",
"metadata": {},
"source": [
"NLPソルバーに\n",
"- 最適化変数の初期値(w0)\n",
"- 最適化変数の上下界(lbw, ubw)\n",
"- 制約の上下界(lbg, ubg)\n",
"を渡して解きます．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "d5021bb3",
"metadata": {
"scrolled": false
},
"outputs": [],
"source": [
"# NLPを解く\n",
"sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)"
]
},
{
"cell_type": "markdown",
"id": "018e6f73",
"metadata": {},
"source": [
"最適解をソルバーから取得し，解をプロットします． \n",
"$\\theta$ が $\\pi$ に，$y$が$0$にそれぞれ収束していっていることがわかります．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "60d69e9b",
"metadata": {},
"outputs": [],
"source": [
"w_opt = sol['x'].full().flatten()\n",
"\n",
"# 解をプロット\n",
"x1_opt = w_opt[0::5]\n",
"x2_opt = w_opt[1::5]\n",
"x3_opt = w_opt[2::5]\n",
"x4_opt = w_opt[3::5]\n",
"u_opt = w_opt[4::5]\n",
"\n",
"tgrid = [dt*k for k in range(N+1)]\n",
"import matplotlib.pyplot as plt\n",
"plt.figure(1)\n",
"plt.clf()\n",
"plt.plot(tgrid, x1_opt, '--')\n",
"plt.plot(tgrid, x2_opt, '-')\n",
"plt.plot(tgrid, x3_opt, '-')\n",
"plt.plot(tgrid, x4_opt, '-')\n",
"plt.step(tgrid, np.concatenate([np.array([np.nan]), u_opt]), '-.')\n",
"plt.xlabel('t')\n",
"plt.legend(['y(x1)','th(x2)', 'dy(x3)', 'dth(x4)','u'])\n",
"plt.grid()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"id": "6f8d9beb",
"metadata": {},
"source": [
"## 5. モデル予測制御（MPC）\n",
"MPCのアイデアはいたって簡単です．\n",
"毎時刻 $t$ でシステムの状態 ${x} (t)$ を計測 or 推定して，それを元に時間 $[t, t+T]$ での最適制御問題\n",
"$$\n",
"\\min_{u} {J} = V_f (x(t+T)) + \\int_{t}^{t+T} l (x (\\tau), u (\\tau)) d \\tau \\\\\n",
"{\\rm s.t.} \n",
"\\;\\;\\; x(t) = {x}(t), \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\dot{x} (\\tau) = f(x(\\tau), u(\\tau)), \\;\\;\\; t \\leq \\tau < t +T \\\\\n",
"\\;\\;\\;\\;\\;\\;\\;\\; g(x(\\tau), u(\\tau)) \\leq 0, \\;\\;\\; t \\leq \\tau < t+T \\\\\n",
"\\;\\;\\; g(x(t+T)) \\leq 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\;\\;\\;\\;\\; c(x(\\tau), u(\\tau)) = 0, \\;\\;\\; t \\leq t < t+T \\\\\n",
"\\;\\;\\; c(x(t+T)) = 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"$$\n",
"を解きます．実際には，次の数値最適制御問題\n",
"$$\n",
"\\min_{x_0, ..., x_N, u_0, ..., u_{N-1}} {J} = V_f (x_N) + \\sum_{i=0}^{N-1} l(x_i, u_i) \\Delta \\tau \\\\\n",
"{\\rm s.t.} \\;\\; x_0 = {x}(t), \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; x_i + f(x_i, u_i) \\Delta \\tau - x_{i+1} = 0, \\;\\;\\; i=0, ..., N-1 \\\\\n",
"\\;\\;\\; g(x_i, u_i) \\leq 0, \\;\\;\\; i=0, ..., N-1, \\; \\\\\n",
"\\;\\;\\; g(x_N) \\leq 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"\\;\\;\\; c(x_i, u_i) = 0, \\;\\;\\; i=0, ..., N-1 \\;\\; \\\\\n",
"\\;\\;\\; c(x_N) = 0, \\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\;\\; \\\\\n",
"$$\n",
"を解きます． \n",
"そして，得られた最適な制御入力 $u_0, ..., u_{N-1}$のうち初期値 $u_0$ を実際のシステムに印加します． "
]
},
{
"cell_type": "markdown",
"id": "561ee98e",
"metadata": {},
"source": [
"### CasADiによる実装\n",
"では実際にCasADiを使って実装してみましょう．\n",
"以下がクラスによる実装ですが，それぞれ次のような役割になっています．\n",
"- `__init__`: コンストラクタ．上の最適制御の問題設定と全く同じです．\n",
"- `init`: MPCソルバーを初期化します．\n",
"- `solve`: 状態 xを与え，それについての最適制御問題を解きます．\n",
"MPCで前のサンプリング時刻での最適解が，次の時刻での良い初期推定解になるという特徴があります(warm start と呼ばれます) ． \n",
"そのため， x, lam_g, lam_x といった変数で解を保存し，solve()関数で再利用しています．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "995c5d3f",
"metadata": {},
"outputs": [],
"source": [
"class MPC:\n",
" def __init__(self):\n",
" # 問題設定\n",
" T = 1.0 # ホライゾン長さ (MPCなので短め)\n",
" N = 20 # ホライゾン離散化グリッド数 (MPCなので荒め)\n",
" dt = T / N # 離散化ステップ\n",
" nx = 4 # 状態空間の次元\n",
" nu = 1 # 制御入力の次元\n",
" cartpole = CartPole() # cartpole のダイナミクス\n",
" cost_function = CostFunction() # コスト関数\n",
"\n",
" # 以下で非線形計画問題(NLP)を定式化\n",
" w = [] # 最適化変数を格納する list\n",
" w0 = [] # 最適化変数(w)の初期推定解を格納する list\n",
" lbw = [] # 最適化変数(w)の lower bound を格納する list\n",
" ubw = [] # 最適化変数(w)の upper bound を格納する list\n",
" J = 0 # コスト関数 \n",
" g = [] # 制約（等式制約，不等式制約どちらも）を格納する list\n",
" lbg = [] # 制約関数(g)の lower bound を格納する list\n",
" ubg = [] # 制約関数(g)の upper bound を格納する list\n",
" lam_x0 = [] # 制約 lbw<w<ubw のラグランジュ乗数\n",
" lam_g0 = [] # 制約 lbg<g<ubg のラグランジュ乗数\n",
"\n",
" Xk = MX.sym('X0', nx) # 初期時刻の状態ベクトル x0\n",
" w += [Xk] # x0 を 最適化変数 list (w) に追加\n",
" # 初期状態は given という条件を等式制約として考慮\n",
" lbw += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" ubw += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" w0 += [0, 0, 0, 0] # x0 の初期推定解\n",
" lam_x0 += [0, 0, 0, 0] # ラグランジュ乗数の初期推定解\n",
"\n",
" # 離散化ステージ 0~N-1 までのコストと制約を設定\n",
" for k in range(N):\n",
" Uk = MX.sym('U_' + str(k), nu) # 時間ステージ k の制御入力 uk を表す変数\n",
" w += [Uk] # uk を最適化変数 list に追加\n",
" lbw += [-15.0] # uk の lower-bound\n",
" ubw += [15.0] # uk の upper-bound\n",
" w0 += [0] # uk の初期推定解\n",
" lam_x0 += [0] # ラグランジュ乗数の初期推定解\n",
"\n",
" # ステージコスト\n",
" J = J + dt * cost_function.stage_cost(Xk, Uk) # コスト関数にステージコストを追加\n",
"\n",
" # Forward Euler による離散化状態方程式\n",
" dXk = cartpole.dynamics(Xk, Uk)\n",
" Xk_next = vertcat(Xk[0] + dXk[0] * dt, \n",
" Xk[1] + dXk[1] * dt,\n",
" Xk[2] + dXk[2] * dt,\n",
" Xk[3] + dXk[3] * dt)\n",
" Xk1 = MX.sym('X_' + str(k+1), nx) # 時間ステージ k+1 の状態 xk+1 を表す変数\n",
" w += [Xk1] # xk+1 を最適化変数 list に追加\n",
" lbw += [-inf, -inf, -inf, -inf] # xk+1 の lower-bound （指定しない要素は -inf）\n",
" ubw += [inf, inf, inf, inf] # xk+1 の upper-bound （指定しない要素は inf）\n",
" w0 += [0.0, 0.0, 0.0, 0.0] # xk+1 の初期推定解\n",
" lam_x0 += [0, 0, 0, 0] # ラグランジュ乗数の初期推定解\n",
"\n",
" # 状態方程式(xk+1=xk+fk*dt) を等式制約として導入\n",
" g += [Xk_next-Xk1]\n",
" lbg += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" ubg += [0, 0, 0, 0] # 等式制約は lower-bound と upper-bound を同じ値にすることで設定\n",
" lam_g0 += [0, 0, 0, 0] # ラグランジュ乗数の初期推定解\n",
" Xk = Xk1\n",
"\n",
" # 終端コスト \n",
" J = J + cost_function.terminal_cost(Xk) # コスト関数に終端コストを追加\n",
"\n",
" self.J = J\n",
" self.w = vertcat(*w)\n",
" self.g = vertcat(*g)\n",
" self.x = w0\n",
" self.lam_x = lam_x0\n",
" self.lam_g = lam_g0\n",
" self.lbx = lbw\n",
" self.ubx = ubw\n",
" self.lbg = lbg\n",
" self.ubg = ubg\n",
"\n",
" # 非線形計画問題(NLP)\n",
" self.nlp = {'f': self.J, 'x': self.w, 'g': self.g} \n",
" # Ipopt ソルバー，最小バリアパラメータを0.1，最大反復回数を5, ウォームスタートをONに\n",
" self.solver = nlpsol('solver', 'ipopt', self.nlp, {'calc_lam_p':True, 'calc_lam_x':True, 'print_time':False, 'ipopt':{'max_iter':5, 'mu_min':0.1, 'warm_start_init_point':'yes', 'print_level':0, 'print_timing_statistics':'no'}}) \n",
" # self.solver = nlpsol('solver', 'scpgen', self.nlp, {'calc_lam_p':True, 'calc_lam_x':True, 'qpsol':'qpoases', 'print_time':False, 'print_header':False, 'max_iter':5, 'hessian_approximation':'gauss-newton', 'qpsol_options':{'print_out':False, 'printLevel':'none'}}) # print をオフにしたいがやり方がわからない\n",
"\n",
"\n",
" def init(self, x0=None):\n",
" if x0 is not None:\n",
" # 初期状態についての制約を設定\n",
" self.lbx[0:4] = x0\n",
" self.ubx[0:4] = x0\n",
" # primal variables (x) と dual variables（ラグランジュ乗数）の初期推定解も与えつつ solve（warm start）\n",
" sol = self.solver(x0=self.x, lbx=self.lbx, ubx=self.ubx, lbg=self.lbg, ubg=self.ubg, lam_x0=self.lam_x, lam_g0=self.lam_g)\n",
" # 次の warm start のために解を保存\n",
" self.x = sol['x'].full().flatten()\n",
" self.lam_x = sol['lam_x'].full().flatten()\n",
" self.lam_g = sol['lam_g'].full().flatten()\n",
"\n",
" def solve(self, x0):\n",
" # 初期状態についての制約を設定\n",
" nx = x0.shape[0]\n",
" self.lbx[0:nx] = x0\n",
" self.ubx[0:nx] = x0\n",
" # primal variables (x) と dual variables（ラグランジュ乗数）の初期推定解も与えつつ solve（warm start）\n",
" sol = self.solver(x0=self.x, lbx=self.lbx, ubx=self.ubx, lbg=self.lbg, ubg=self.ubg, lam_x0=self.lam_x, lam_g0=self.lam_g)\n",
" # 次の warm start のために解を保存\n",
" self.x = sol['x'].full().flatten()\n",
" self.lam_x = sol['lam_x'].full().flatten()\n",
" self.lam_g = sol['lam_g'].full().flatten()\n",
" return np.array([self.x[4]]) # 制御入力を return\n"
]
},
{
"cell_type": "markdown",
"id": "4d538752",
"metadata": {},
"source": [
"### 数値シミュレーション\n",
"実際にMPCソルバーを用いて数値シミュレーションをしてみます．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "8cca1d23",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"import math\n",
"sim_time = 10.0 # 10秒間のシミュレーション\n",
"sampling_time = 0.01 # 0.01秒（10ms）のサンプリング周期\n",
"sim_steps = math.floor(sim_time/sampling_time)\n",
"xs = []\n",
"us = []\n",
"cartpole = CartPole()\n",
"mpc = MPC()\n",
"mpc.init()\n",
"x = np.zeros(4)\n",
"for step in range(sim_steps):\n",
" if step%(1/sampling_time)==0:\n",
" print('t =', step*sampling_time)\n",
" u = mpc.solve(x)\n",
" xs.append(x)\n",
" us.append(u)\n",
" x1 = x + sampling_time * np.array(cartpole.dynamics(x, u))\n",
" x = x1\n",
"\n",
"# シミュレーション結果をプロット\n",
"xs1 = [x[0] for x in xs]\n",
"xs2 = [x[1] for x in xs]\n",
"xs3 = [x[2] for x in xs]\n",
"xs4 = [x[3] for x in xs]\n",
"tgrid = [sampling_time*k for k in range(sim_steps)]\n",
"\n",
"import matplotlib.pyplot as plt\n",
"plt.figure(1)\n",
"plt.clf()\n",
"plt.plot(tgrid, xs1, '--')\n",
"plt.plot(tgrid, xs2, '-')\n",
"plt.plot(tgrid, xs3, '-')\n",
"plt.plot(tgrid, xs4, '-')\n",
"plt.step(tgrid, us, '-.')\n",
"plt.xlabel('t')\n",
"plt.legend(['y(x1)','th(x2)', 'dy(x3)', 'dth(x4)','u'])\n",
"plt.grid()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"id": "a0a6e240",
"metadata": {},
"source": [
"## 6. まとめ\n",
"数値最適制御の簡単な定式化から，MPCまでの実装を紹介しました． \n",
"CasADiは非常に便利なツールです． また今回バックエンドで用いたIpoptは難しいNLPもロバストに解いてくれる素晴らしいソルバーです． \n",
"一方で，上記のように汎用ソルバーIpoptをそのまま使用するだけでは計算が重たいことなど実感できるかと思います． \n",
"そのため，数値最適制御アルゴリズムの高速化はMPC研究の1大テーマになっています． \n",
"\n",
"\n",
"ここまで具体的なアルゴリズムについては一切触れていませんでした．具体的な数値最適制御の理論やアルゴリズムが気になった方は，是非 \n",
"- [Sebastien Gros and Moritz M. Diehl, Numerical Optimial Control](https://www.syscop.de/files/2020ss/NOC/book-NOCSE.pdf)\n",
"- [James B. Rawlings, David Q. Mayne, and Moritz M. Dieh, Model Predictive Control: Theory, Computation, and Design, 2nd section, Nob Hill Publishing](https://sites.engineering.ucsb.edu/~jbraw/mpc/) (特に Chapter 8) \n",
"\n",
"を，数値最適化全般に興味のある方は是非\n",
"\n",
"- [Jorge Nocedal, Stephen J. Wright, Numerical Optimization, Springer](http://www.apmath.spbu.ru/cnsa/pdf/monograf/Numerical_Optimization2006.pdf) \n",
"\n",
"を読んでみてください．\n"
]
},
{
"cell_type": "markdown",
"id": "0efc42b9",
"metadata": {},
"source": [
"## 7. (Advanced) Open source MPCソルバー acados\n",
"より高速に最適制御問題を解くために，[acados](https://github.com/acados/acados)というツールを用いた例を紹介します． \n",
"acados は CasADiで問題を記述するインターフェースを持ち，内部では最適制御(MPC)に特化したソルバーを呼び出してくれます． \n",
"まずは以下のコマンドをターミナルに打ち込んでインストールしましょう（ubuntu, macでテスト済み．windowsは[公式ドキュメント](https://docs.acados.org/installation/index.html)参照） \n",
"Ubuntuの場合は\n",
"```\n",
"git clone https://github.com/acados/acados\n",
"cd acados\n",
"git submodule update --recursive --init\n",
"mkdir -p build\n",
"cd build\n",
"cmake .. -DCMAKE_BUILD_TYPE=Release\n",
"make -j4\n",
"sudo make install -j\n",
"cd ..\n",
"pip3 install -e interfaces/acados_template\n",
"echo export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$PWD/lib >> ~/.bashrc\n",
"echo export ACADOS_SOURCE_DIR=$PWD >> ~/.bashrc\n",
"source ~/.bashrc\n",
"```\n",
"Macの場合は\n",
"```\n",
"git clone https://github.com/acados/acados\n",
"cd acados\n",
"git submodule update --recursive --init\n",
"mkdir -p build\n",
"cd build\n",
"cmake .. -DCMAKE_BUILD_TYPE=Release\n",
"make -j4\n",
"sudo make install -j\n",
"cd ..\n",
"pip3 install -e interfaces/acados_template\n",
"echo export DYLD_LIBRARY_PATH=$DYLD_LIBRARY_PATH:$PWD/lib >> ~/.zshrc\n",
"echo export ACADOS_SOURCE_DIR=$PWD >> ~/.zshrc\n",
"source ~/.zshrc\n",
"```\n",
"をターミナルにペーストしてください． \n",
"インストール後に以降のコードがうまくいかない場合は Jupyter Notebookを再起動してください．\n",
"\n",
"インストールできたら，以下で必要なモジュールをインポートしましょう"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "9fb05545",
"metadata": {},
"outputs": [],
"source": [
"from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel"
]
},
{
"cell_type": "markdown",
"id": "4d462c8f",
"metadata": {},
"source": [
"まずはダイナミクスのモデル `AcadosModel` を定義します．（以前に作成した class`CartPole`を再利用します．）"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "9236174b",
"metadata": {},
"outputs": [],
"source": [
"model = AcadosModel() # 状態，制御入力，ダイナミクスを CasADiの symbolic expressionを用いて記述\n",
"y = SX.sym('y')\n",
"th = SX.sym('th')\n",
"dy = SX.sym('dy')\n",
"dth = SX.sym('dth')\n",
"x = vertcat(y, th, dy, dth)\n",
"u = vertcat(SX.sym('f'))\n",
"\n",
"y_dot = SX.sym('y_dot')\n",
"th_dot = SX.sym('th_dot')\n",
"dy_dot = SX.sym('dy_dot')\n",
"dth_dot = SX.sym('dth_dot')\n",
"xdot = vertcat(y_dot, th_dot, dy_dot, dth_dot)\n",
"\n",
"cartpole = CartPole()\n",
"dy, dth, ddy, ddth, = cartpole.dynamics(x, u) # 状態方程式\n",
"f = vertcat(dy, dth, ddy, ddth)\n",
"\n",
"model.x = x\n",
"model.u = u\n",
"model.p = [] #時変パラメータ？\n",
"model.z = []\n",
"cartpole = CartPole()\n",
"model.f_expl_expr = f # 状態方程式\n",
"model.f_impl_expr = xdot - f # DAEモデルの記述用？普通の x\\dot = f(x, u) は左のように記述\n",
"model.xdot = xdot # xdot の CasADi symbols\n",
"model.name = f'cart_pole'"
]
},
{
"cell_type": "markdown",
"id": "5222cc84",
"metadata": {},
"source": [
"次に最適制御問題（ホライゾン長さ，評価関数，制約）を`AcadosOcp`で 定義していきます． \n",
"acadosでは評価関数の設定方法に多少特徴があり，今回は2次形式関数\n",
"$$V_f = \\frac{1}{2} (x - x_{\\rm ref})^{\\rm T} Q_{f} (x - x_{\\rm ref}) ,$$ \n",
"およびステージコスト\n",
"$$l(t, x, u) = \\frac{1}{2} (x - x_{\\rm ref})^{\\rm T} Q (x - x_{\\rm ref}) + \\frac{1}{2} u^{\\rm T} R u $$ \n",
"を考えたいわけですが，これをacadoの2次形式関数\n",
"$$V_f = \\frac{1}{2} (V_{x_e} x - y_{e_{\\rm ref}})^{\\rm T} W_e (V_{x_e} x - y_{e_{\\rm ref}}) ,$$ \n",
"およびステージコスト\n",
"$$l(t, x, u) = \\frac{1}{2} (\\begin{bmatrix} V_x & V_u \\end{bmatrix} \\begin{bmatrix} x \\\\ u \\end{bmatrix} - y_{\\rm ref})^{\\rm T} W (\\begin{bmatrix} V_x & V_u \\end{bmatrix} \\begin{bmatrix} x \\\\ u \\end{bmatrix} - y_{\\rm ref}) $$ \n",
"の形式に設定していきます．（当然ですが，acadosには他の一般的な評価関数も組み込めます．）"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "3f2cb69b",
"metadata": {},
"outputs": [],
"source": [
"## 最適制御問題\n",
"ocp = AcadosOcp()\n",
"ocp.solver_options.tf = 5.0 # ホライゾン長さ\n",
"ocp.dims.N = 100 # ホライゾン離散化分割数\n",
"ocp.model = model # 作成したAcadosModel() を使用\n",
"\n",
"## 評価関数\n",
"ocp.cost.cost_type = 'LINEAR_LS' # ステージコストを2次形式に指定\n",
"ocp.cost.cost_type_e = 'LINEAR_LS' # 終端コストを2次形式に指定\n",
"nx = ocp.model.x.size()[0]\n",
"nu = ocp.model.u.size()[0]\n",
"ny = nx + nu\n",
"\n",
"cost_function = CostFunction() # 以前作成したクラス CostFunctionを再利用\n",
"\n",
"ocp.cost.W_e = np.diag(cost_function.Qf) # 終端コストのヘッセ行列\n",
"ocp.cost.W = np.diag(np.concatenate([cost_function.Q, cost_function.R])) # ステージコスト\n",
"\n",
"ocp.cost.Vx = np.zeros((ny, nx))\n",
"ocp.cost.Vx[:nx,:nx] = np.eye(nx)\n",
"\n",
"Vu = np.zeros((ny, nu))\n",
"Vu[4,0] = 1.0\n",
"ocp.cost.Vu = Vu\n",
"\n",
"ocp.cost.Vx_e = np.eye(nx)\n",
"\n",
"ocp.cost.yref = np.concatenate([cost_function.x_ref, np.zeros(1)])\n",
"ocp.cost.yref_e = np.concatenate([cost_function.x_ref])\n",
"\n",
"\n",
"## 制約\n",
"ocp.constraints.x0 = np.array([0.0, 0.0, 0.0, 0.0]) #初期時刻の状態を制約として指定\n",
"\n",
"ocp.constraints.constr_type = 'BGH'\n",
"ocp.constraints.lbu = np.array([-5.0]) #制御入力の下限\n",
"ocp.constraints.ubu = np.array([5.0]) #制御入力の上限\n",
"ocp.constraints.idxbu = np.array([0]) #制御入力の制約をかけるインデックス\n",
"\n",
"ocp.constraints.lbx = np.array([-10.0]) #状態の下限\n",
"ocp.constraints.ubx = np.array([10.0]) #状態の上限\n",
"ocp.constraints.idxbx = np.array([0]) #状態の制約をかけるインデックス\n",
"\n",
"ocp.constraints.constr_type_e = 'BGH'\n",
"ocp.constraints.lbx_e = np.array([-10.0]) #終端状態の下限\n",
"ocp.constraints.ubx_e = np.array([10.0]) #終端状態の上限\n",
"ocp.constraints.idxbx_e = np.array([0]) #終端状態の制約をかけるインデックス"
]
},
{
"cell_type": "markdown",
"id": "87ca00fa",
"metadata": {},
"source": [
"ではいよいよソルバー `AcadosOcpSolver`を作成し，最適制御問題を解きます．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "19226212",
"metadata": {},
"outputs": [],
"source": [
"ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # Riccati recursion\n",
"ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' \n",
"ocp.solver_options.integrator_type = 'ERK'\n",
"ocp.solver_options.sim_method_num_stages = 1 # ERK + 1 でオイラー法， ERK + 4 でよくある runge-kutta法\n",
"ocp.solver_options.globalization = 'MERIT_BACKTRACKING'\n",
"ocp.solver_options.nlp_solver_max_iter = 200\n",
"# ocp.solver_options.print_level = 1\n",
"ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP\n",
"ocp.solver_options.hpipm_mode = 'SPEED'\n",
"\n",
"from acados_template import ocp_get_default_cmake_builder\n",
"cmake_builder = ocp_get_default_cmake_builder()\n",
"# ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') # Pythonインターフェースでそのまま解く場合\n",
"ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', cmake_builder=cmake_builder) # C++コード生成＋コンパイルして解く場合\n",
"\n",
"status = ocp_solver.solve()\n",
"\n",
"if status != 0:\n",
" ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats(\"statistics\")\n",
" raise Exception(f'acados returned status {status}.')\n",
"\n",
"ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats(\"statistics\")"
]
},
{
"cell_type": "markdown",
"id": "f8ca115e",
"metadata": {},
"source": [
"解をプロットしてみます．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "f5e4237d",
"metadata": {},
"outputs": [],
"source": [
"# 解をプロット\n",
"x1_opt = []\n",
"x2_opt = []\n",
"x3_opt = []\n",
"x4_opt = []\n",
"u_opt = []\n",
"\n",
"for i in range(ocp.dims.N+1):\n",
" x1, x2, x3, x4 = ocp_solver.get(i, \"x\")\n",
" x1_opt.append(x1)\n",
" x2_opt.append(x2)\n",
" x3_opt.append(x3)\n",
" x4_opt.append(x4)\n",
"\n",
"for i in range(ocp.dims.N):\n",
" u1, = ocp_solver.get(i, \"u\")\n",
" u_opt.append(u1)\n",
"\n",
"tgrid = [dt*k for k in range(ocp.dims.N+1)]\n",
"import matplotlib.pyplot as plt\n",
"plt.figure(1)\n",
"plt.clf()\n",
"plt.plot(tgrid, np.array(x1_opt), '--')\n",
"plt.plot(tgrid, np.array(x2_opt), '-')\n",
"plt.plot(tgrid, np.array(x3_opt), '-')\n",
"plt.plot(tgrid, np.array(x4_opt), '-')\n",
"plt.step(tgrid, np.concatenate([np.array([np.nan]), np.array(u_opt)]), '-.')\n",
"plt.xlabel('t')\n",
"plt.legend(['y(x1)','th(x2)', 'dy(x3)', 'dth(x4)','u'])\n",
"plt.grid()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"id": "e0ee2cb0",
"metadata": {},
"source": [
"## TODO: acados によるMPCの実装\n",
"現在のところうまくいっていません．原因は調査中です．"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "9341f1f6",
"metadata": {},
"outputs": [],
"source": [
"## MPC用最適制御問題\n",
"mpc = AcadosOcp()\n",
"mpc.solver_options.tf = 1.0 # ホライゾン長さ\n",
"mpc.dims.N = 20 # ホライゾン離散化分割数\n",
"mpc.model = model # 作成したAcadosModel() を使用\n",
"\n",
"## 評価関数\n",
"mpc.cost.cost_type = 'LINEAR_LS' # ステージコストを2次形式に指定\n",
"mpc.cost.cost_type_e = 'LINEAR_LS' # 終端コストを2次形式に指定\n",
"nx = mpc.model.x.size()[0]\n",
"nu = mpc.model.u.size()[0]\n",
"ny = nx + nu\n",
"\n",
"cost_function = CostFunction() # 以前作成したクラス CostFunctionを再利用\n",
"\n",
"mpc.cost.W_e = np.diag(cost_function.Qf) # 終端コストのヘッセ行列\n",
"mpc.cost.W = np.diag(np.concatenate([cost_function.Q, cost_function.R])) # ステージコスト\n",
"\n",
"mpc.cost.Vx = np.zeros((ny, nx))\n",
"mpc.cost.Vx[:nx,:nx] = np.eye(nx)\n",
"\n",
"Vu = np.zeros((ny, nu))\n",
"Vu[4,0] = 1.0\n",
"mpc.cost.Vu = Vu\n",
"\n",
"mpc.cost.Vx_e = np.eye(nx)\n",
"\n",
"mpc.cost.yref = np.concatenate([cost_function.x_ref, np.zeros(1)])\n",
"mpc.cost.yref_e = np.concatenate([cost_function.x_ref])\n",
"\n",
"\n",
"## 制約\n",
"mpc.constraints.x0 = np.array([0.0, 0.0, 0.0, 0.0]) #初期時刻の状態を制約として指定\n",
"\n",
"mpc.constraints.constr_type = 'BGH'\n",
"mpc.constraints.lbu = np.array([-15.0]) #制御入力の下限\n",
"mpc.constraints.ubu = np.array([15.0]) #制御入力の上限\n",
"mpc.constraints.idxbu = np.array([0]) #制御入力の制約をかけるインデックス"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "b13b7897",
"metadata": {
"scrolled": true
},
"outputs": [],
"source": [
"mpc.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # Riccati recursion\n",
"mpc.solver_options.hessian_approx = 'GAUSS_NEWTON' \n",
"mpc.solver_options.integrator_type = 'ERK'\n",
"ocp.solver_options.sim_method_num_stages = 1 # ERK + 1 でオイラー法， ERK + 4 でよくある runge-kutta法\n",
"# mpc.solver_options.globalization = 'FIXED_STEP' # 直線探索をオフに\n",
"mpc.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP # 反復回数1回（nlp_solver_max_iter = 1）であれば SQP_RTIが有用．詳しくは\"Real-time iteration scheme\"で検索．\n",
"# mpc.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # 反復回数1回（nlp_solver_max_iter = 1）であれば SQP_RTIが有用．詳しくは\"Real-time iteration scheme\"で検索．\n",
"mpc.solver_options.nlp_solver_max_iter = 5 # 反復回数をリアルタイム計算可能な程度に固定\n",
"mpc.solver_options.qp_solver_iter_max = 5 # 反復回数をリアルタイム計算可能な程度に固定\n",
"mpc.solver_options.hpipm_mode = 'SPEED'\n",
"\n",
"from acados_template import ocp_get_default_cmake_builder\n",
"cmake_builder = ocp_get_default_cmake_builder()\n",
"# mpc_solver = AcadosOcpSolver(mpc, json_file = 'acados_mpc.json') # Pythonで解く場合\n",
"mpc_solver = AcadosOcpSolver(mpc, json_file = 'acados_mpc.json', cmake_builder=cmake_builder) # CMakeでコード生成＋解く場合\n",
"# mpc_solver.options_set('qp_tau_min', 1.0e-01)\n",
"# mpc_solver.options_set('warm_start_first_qp', 1)\n",
"# mpc_solver.options_set('qp_tol_stat', 1.0e-04)\n",
"# mpc_solver.options_set('qp_tol_eq', 1.0e-04)\n",
"# mpc_solver.options_set('qp_tol_ineq', 1.0e-04)\n",
"# mpc_solver.options_set('qp_tol_comp', 1.0e-04)\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "735ab640",
"metadata": {},
"outputs": [],
"source": [
"# import math\n",
"sim_time = 10.0 # 10秒間のシミュレーション\n",
"sampling_time = 0.01 # 0.01秒（10ms）のサンプリング周期\n",
"sim_steps = math.floor(sim_time/sampling_time)\n",
"xs = []\n",
"us = []\n",
"cartpole = CartPole()\n",
"\n",
"x = np.zeros(4)\n",
"mpc_solver.constraints_set(0, \"lbx\", x)\n",
"mpc_solver.constraints_set(0, \"ubx\", x)\n",
"\n",
"WARM_START_ITERS = 100\n",
"for i in range(WARM_START_ITERS):\n",
" mpc_solver.solve()\n",
"\n",
"for step in range(sim_steps):\n",
" if step%(1/sampling_time)==0:\n",
" print('t =', step*sampling_time) \n",
" # 初期状態を更新\n",
" mpc_solver.constraints_set(0, \"lbx\", x)\n",
" mpc_solver.constraints_set(0, \"ubx\", x)\n",
" status = mpc_solver.solve()\n",
" # 制御入力をソルバーから取得\n",
" u = mpc_solver.get(0, \"u\")\n",
" xs.append(x)\n",
" us.append(u)\n",
" x1 = x + sampling_time * np.array(cartpole.dynamics(x, u))\n",
" x = x1"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "6c0b4c52",
"metadata": {},
"outputs": [],
"source": [
"# シミュレーション結果をプロット\n",
"xs1 = [x[0] for x in xs]\n",
"xs2 = [x[1] for x in xs]\n",
"xs3 = [x[2] for x in xs]\n",
"xs4 = [x[3] for x in xs]\n",
"tgrid = [sampling_time*k for k in range(sim_steps)]\n",
"\n",
"import matplotlib.pyplot as plt\n",
"plt.figure(1)\n",
"plt.clf()\n",
"plt.plot(tgrid, xs1, '--')\n",
"plt.plot(tgrid, xs2, '-')\n",
"plt.plot(tgrid, xs3, '-')\n",
"plt.plot(tgrid, xs4, '-')\n",
"plt.step(tgrid, us, '-.')\n",
"plt.xlabel('t')\n",
"plt.legend(['y(x1)','th(x2)', 'dy(x3)', 'dth(x4)','u'])\n",
"plt.grid()\n",
"plt.show()"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.9.13"
}
},
"nbformat": 4,
"nbformat_minor": 5
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment