design_controller.ipynb

design_controller.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/gist/maruta/89fbc1eef0a7a391b3268bd7e53783e4/design_controller.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cDLWeRUH6zgY"
      },
      "source": [
        "# 極配置による制御系の設計\n",
        "\n",
        "ここでは Python の control パッケージを使い、極配置法によって状態フィードバック制御器を設計する．\n",
        "\n",
        "**TODO**　x になっている部分を適切に穴埋めして実行し、制御器を設計せよ。"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "tl1IC7Xu6zgb"
      },
      "outputs": [],
      "source": [
        "!pip install control\n",
        "import matplotlib.pyplot as plt\n",
        "import numpy as np\n",
        "from control.matlab import *\n",
        "from sympy import *\n",
        "from sympy.physics.mechanics import *\n",
        "from google.colab import files\n",
        "init_vprinting(use_latex=\"mathjax\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3A_CrDVh6zgf"
      },
      "source": [
        "### モデルの構築\n",
        "制御系設計に使うモデルのシステム行列$A,B$を作る"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "CRgKVSed6zgf"
      },
      "outputs": [],
      "source": [
        "# 定数類\n",
        "x=1\n",
        "mb = x\n",
        "Jb = x\n",
        "mw = x\n",
        "Jw = x\n",
        "l = x\n",
        "r = x\n",
        "g = 9.8\n",
        "\n",
        "# 代入\n",
        "A = np.array(\n",
        "    [[x,x,x,x],\n",
        "     [x,x,x,x],\n",
        "     [x,x,x,x],\n",
        "     [x,x,x,x]], dtype=float)\n",
        "B = np.array(\n",
        "    [[x],\n",
        "     [x],\n",
        "     [x],\n",
        "     [x]], dtype=float)\n",
        "\n",
        "# 表示\n",
        "display(A)\n",
        "display(B)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Wc_Qpuqi6zgf"
      },
      "source": [
        "## 極配置による制御系設計\n",
        "\n",
        "ここでは状態変数 $x = [x_b, \\theta_b, \\dot{x}_b, \\dot{\\theta}_b]^\\top$ にゲイン $K = [K_{x_b}, K_{\\theta_b}, K_{\\dot{x}_b}, K_{\\dot{\\theta}_b}]$ を乗じて制御入力を\n",
        "$$\n",
        " u = -Kx\n",
        "$$\n",
        "と定める状態フィードバックによる制御を試みる．\n",
        "\n",
        "このとき，対象システムのモデル\n",
        "$$\n",
        " \\dot{x} = Ax+Bu\n",
        "$$\n",
        "に$u=-Kx$を代入すると，状態フィードバックの下でシステムの運動は微分方程式\n",
        "$$\n",
        " \\dot{x} = (A-BK)x \\tag{1}\n",
        "$$\n",
        "に従うことがわかる．この微分方程式の解が\n",
        "$$\n",
        " \\lim_{t\\to\\infty} x= 0\n",
        "$$\n",
        "であれば，線形モデルにおいては倒立が実現できることになる．\n",
        "\n",
        "(1)の解が0に収束する条件は $A-BK$ の固有値の実部がすべて負であることである．\n",
        "今，$A$と$B$は対象システムの構造から前節で求めたように決まっているので，\n",
        "$K$を適切に設計して$A-BK$の固有値の実部がすべて負になるように設計することがここでの問題である．\n",
        "\n",
        "そのための方法の一つとして極配置法が知られている．これはまず，望ましい$A-BK$の固有値を設計者が定め，それを実現する$K$を逆算する方法である．\n",
        "線形システムにおいてダイナミクスを表す行列（ここでは$A-BK$のこと）の固有値を制御分野では古典制御における伝達関数の極との対応から慣例的に「極」と呼んでおり，この方法は極を望みの位置に配置する方法であるので「極配置法」と呼ばれている．\n",
        "\n",
        "逆算の手順としてはアッカーマンアルゴリズムなどが知られている。ここでは Python Control Systems Library に含まれる実装を用いて以下のようにゲイン$K$を計算する．\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "mEPIbwwp6zgf"
      },
      "outputs": [],
      "source": [
        "# TODO:望ましい極を定めよ\n",
        "# 複素数は 1+2j のように表現できる\n",
        "# 複素数の極は必ず共役のペアになっていなければならない\n",
        "\n",
        "pl = [x, x, x, x]\n",
        "K = acker(A, B, plt)\n",
        "\n",
        "# 計算されたゲインを表示．\n",
        "# 制御用のコードにコピペすること\n",
        "\n",
        "print(f\"\"\"\n",
        "const double K[4] = {{{K[0,0]},{K[0,1]}, {K[0,2]}, {K[0,3]}}};\n",
        "\"\"\")\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Fy05Y38H6zgf"
      },
      "source": [
        "念のため，実際に計算されたゲイン$K$が指定された極を実現しているかどうかを確認する．"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "RIJpQ5OH6zgf"
      },
      "outputs": [],
      "source": [
        "eigen_values, eigen_vectors = np.linalg.eig(Am - Bm @ K)\n",
        "display(eigen_values)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "NxaZcCRu6zgf"
      },
      "source": [
        "さらに，線形モデルにおける初期値応答（$x_0=[\\varphi,\\theta,\\dot{\\varphi},\\dot{\\theta}]^\\top=[0,-1,0,0]^\\top$から制御を行ったときの応答）を以下のように計算できる．"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "ubMhPOIL6zgf"
      },
      "outputs": [],
      "source": [
        "P = ss(Am, Bm, eye(4), 0)\n",
        "\n",
        "xout, T = initial(feedback(P, K), X0=[0, -1, 0, 0])\n",
        "\n",
        "fig, axes = plt.subplots(nrows=4, sharex=True, figsize=(8, 12))\n",
        "\n",
        "labels = [r\"$\\varphi$ [rad]\", r\"$\\theta$ [rad]\", r\"$\\dot{\\varphi}$ [rad/s]\", r\"$\\dot{\\theta}$ [rad/s]\"]\n",
        "\n",
        "for k in range(4):\n",
        "    axes[k].plot(T, xout[:, k])\n",
        "    axes[k].set_ylabel(labels[k])\n",
        "    axes[k].grid(True)\n",
        "    axes[k].set_xlim(T[0], T[-1])\n",
        "\n",
        "axes[-1].set_xlabel(r\"$t$ [s]\")\n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "source": [
        "結果をファイルに保存"
      ],
      "metadata": {
        "id": "3CKUmdoPNrER"
      }
    },
    {
      "cell_type": "code",
      "source": [
        "pl = [-1.2j,1.0,2.1,3]\n",
        "\n",
        "poles_str = [f\"({p.real:.1f},{p.imag:.1f}i)\" if p.imag else f\"{p.real:.1f}\" for p in pl]\n",
        "filename = f\"poles_{'_'.join(poles_str)}.tsv\"\n",
        "\n",
        "header = \"K = \"+np.array2string(K, separator=', ')+\", pl = \"+np.array2string(np.array(pl), separator=', ')\n",
        "\n",
        "np.savetxt(\n",
        "    filename,\n",
        "    np.hstack((T[:,np.newaxis], xout)), header=header,delimiter=\"\\t\")\n",
        "\n",
        "files.download(filename)"
      ],
      "metadata": {
        "id": "s3W2xAKfNpXh"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "filename"
      ],
      "metadata": {
        "id": "3J8c9kn3IYI5"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [],
      "metadata": {
        "id": "kkId8NiEQeOT"
      },
      "execution_count": null,
      "outputs": []
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "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.8.6"
    },
    "colab": {
      "provenance": [],
      "include_colab_link": true
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}