コンテンツへスキップ

本記事は学校の課題をプログラムで楽してこなそうとした際のメモを編集したものです。

2020/04/11
本記事は学校の課題をプログラムで楽してこなそうとした際のメモを編集したものです。レポートの面影はありません。

目次

  • 運動方程式を求める
    • モデル
    • 式変形用コード
    • 運動方程式
  • シミュレーションしてみる
    • 順運動学、逆運動学
    • 順逆動力学計算用コード
    • 特異点
  • 雑記

運動方程式を求める

※参考1-1のままやって一致すれば正しく出来ました、ということにしています。

モデル

2リンクマニピュレータ
2リンクマニピュレータ。x-y軸の原点をアームの根元に一致させた。

上図のような2リンクマニピュレータを考えます。G_0=(x_0,y_0), G_1=(x_0,y_1), m_0, m_1, l_0, l_1をそれぞれ第一リンク、第二リンクの重心、質量、長さとします。また、リンク端からそれぞれの重心までの長さをそれぞれl_g0, l_g1としています。

$$l_{gi}=\frac{1}{2}l_i  (i=0,1)$$

ラグランジュの運動方程式に当てはめて運動方程式を求めます。

ラグランジュの運動方程式:

$$ \frac{d}{dt}(\frac{\partial L}{\partial\dot q_i})-\frac{\partial L}{\partial q_i}= Q_i (i =1,2,..,f)$$


$$L=T-U$$

あるいは、

$$ \frac{d}{dt}(\frac{\partial T}{\partial\dot q})-\frac{\partial T}{\partial q_i}+\frac{\partial U}{\partial q_i}+\frac{\partial D}{\partial \dot{q_i}}= Q_i (i =1,2,..,f)$$

L:ラグランジアン

Q:非保存力による一般化力

f:自由度

q:一般化座標

式変形用コード

ラグランジュ~でやると無限微分地獄で計算ミスのリスクが高いですね。少しの変更でものすごく時間がかかるのは嫌なので、Pythonで式変形する方法をやってみたいと思います。こういうときはmatlabっぽく書けるSimPyが便利 (雑記1.)。

  • 式変形
simplify()
>from sympy import *
>x, y, z = symbols('x y z')
>init_printing(use_unicode=True)
>simplify(sin(x)**2 + cos(x)**2)
1
>simplify((x**3 + x**2 - x - 1)/(x**2 + 2*x + 1))
x - 1
>simplify(gamma(x)/gamma(x - 2))
(x - 2)⋅(x - 1)

式を単純化する関数で、計算量が不必要に大きくなることがあることに注意が必要です。神。

Simplification -SymPy1.5.1

  • 解く
sp.solve (Eq)

引数に=0になる関数を入力します。

Solvers -SymPy 1.5.1

  • 微分
diff(func, t)

第二引数に何で微分するのかを入れます。

Calculus -SymPy 1.5.1

参考=>1-2、1-3、1-4

  • コード

環境:

  • Python 3.7.3
  • numpy 1.16.4
  • sympy 1.4

https://github.com/inomatly/robot_arm/blob/master/solv_lagrange.ipynb

一応.pyも置いてありますが jupyter notebok で見ることを前提にしています。

式を単純化するだけのコードです。計算結果はTEXで出しています。

運動方程式

2リンクマニピュレータ(再)

各リンクのx軸とのなす角τ_0, τ_1は

$$\tau_0=\theta_0 ...(1-1)$$

$$\tau_1=\theta_0+\theta_1 ...(1-2)$$

です。 また、第一リンクの重心位置(x_0, y_0)は

$$\begin{bmatrix}x_0 \cr y_0\end{bmatrix}=\begin{bmatrix}l_{g0}cos\theta_0\cr l_{g0}sin\theta_0\end{bmatrix} ...(1-3)$$

で表せます。また、第二リンクの重心位置(x_1,y_1)は

$$\begin{bmatrix}x_1 \cr y_1\end{bmatrix}=\begin{bmatrix}l_{g0}cos\theta_0+ l_{g1}cos(\theta_0+\theta_1)\cr l_{g0}sin\theta_0+ l_{g1}sin(\theta_0+\theta_1)\end{bmatrix} ...(1-4)$$

で表せる。各リンクの運動エネルギー、位置エネルギー、損失エネルギー、一般化力をT_0, T_1, U_0, U_1, D_0, D_1, Q_0, Q_1、一般化座標q_0, q_1として、ラグランジュの運動方程式

$$ \frac{d}{dt}(\frac{\partial T}{\partial\dot q})-\frac{\partial T}{\partial q_i}+\frac{\partial U}{\partial q_i}+\frac{\partial D}{\partial \dot{q_i}}= Q_i (i =1,2) ...(1-5)$$

を用いて各リンクの運動方程式を求める。

ここで、一般化座標はq_i=θ_i (i=0,1)である。各リンクの慣性モーメントをI_0, I_1,関節の粘性摩擦係数をd_0, d_1としてT_0, T_1, U_0, U_1, D_0, D_1を求める。

$$T_0=\frac{1}{2} \left(I_{0} + l_{g0}^{2} m_{0}\right) \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} ...(1-6)$$

$$U_0=g l_{g0} m_{0} \sin{\left({q_{0}}{\left(t \right)} \right)} ...(1-7)$$

$$D_0=\frac{d_{0} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2}}{2} ...(1-8)$$

$$T_1=\frac{1}{2} I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ \frac{1}{2} m_{1} \left(l_{0}^{2} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} + 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} \\+ 2 l_{g1}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2}\right) ...(1-9)$$

$$U_1=g m_{1} \left(l_{0} \sin{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \sin{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right) ...(1-10)$$

$$D_1=\frac{d_{0} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2}}{2} ...(1-11)$$

また、全体の運動エネルギー、位置エネルギー、損失エネルギーT, U, Dは

$$T=T_0+T_1 ...(1-12)$$

$$U=U_0+U_1 ...(1-13)$$

$$T=D_0+D_1 ...(1-14)$$

これより、

$$\frac{\partial T}{\partial \dot{q_0}}=I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) + m_{1} \left(l_{0}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d}{d t} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d}{d t} {q_{0}}{\left(t \right)}  ...(1-15)$$

$$\frac{d}{dt}(\frac{\partial T}{\partial \dot{q_0}})=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + m_{1} \left(l_{0}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\- 2 l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} - l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} ...(1-16)$$

$$\frac{\partial T}{\partial q_0}=0 ...(1-17)$$

$$\frac{\partial U}{\partial q_0}=g \left(l_{g0} m_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + m_{1} \left(l_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right)\right) ..(1-18)$$

$$\frac{\partial D}{\partial \dot{q_0}}=d_{0} \frac{d}{d t} {q_{0}}{\left(t \right)} ...(1-19)$$

$$∴ Q_0=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + d_{0} \frac{d}{d t} {q_{0}}{\left(t \right)} \\+ g \left(l_{g0} m_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + m_{1} \left(l_{0} \cos{\left({q_{0}}{\left(t \right)} \right)} + l_{g1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)}\right)\right) \\+ m_{1} \left(l_{0}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} - 2 l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} - l_{0} l_{g1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{1}}{\left(t \right)}\right)^{2} \\+ 2 l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{0} l_{g1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} \\+ l_{g1}^{2} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) +  \left(I_{0} + l_{g0}^{2} m_{0}\right) \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} ...(1-20)$$

$$\frac{\partial T}{\partial \dot{q_1}}=I_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) \\+ l_{g1} m_{1} \left(l_{0} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{g1} \frac{d}{d t} {q_{0}}{\left(t \right)} + l_{g1} \frac{d}{d t} {q_{1}}{\left(t \right)}\right) ...(1-21)$$

$$\frac{d}{dt}(\frac{\partial T}{\partial \dot{q_1}})=I_{1} \left(\frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) + l_{g1} m_{1} \left(- l_{0} \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} \frac{d}{d t} {q_{1}}{\left(t \right)} \\+ l_{0} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)}\right) ...(1-22)$$

$$\frac{\partial T}{\partial q_1}=-  l_{0} l_{g1} m_{1} \left(\frac{d}{d t} {q_{0}}{\left(t \right)} + \frac{d}{d t} {q_{1}}{\left(t \right)}\right) \sin{\left({q_{1}}{\left(t \right)} \right)} \frac{d}{d t} {q_{0}}{\left(t \right)} ...(1-23)$$

$$\frac{\partial U}{\partial q_1}=g l_{g1} m_{1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)} ...(1-24)$$

$$\frac{\partial D}{\partial \dot{q_1}}=d_{0} \frac{d}{d t} {q_{1}}{\left(t \right)} ...(1-25)$$

$$∴ Q_1= I_{1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + I_{1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} + d_{0} \frac{d}{d t} {q_{1}}{\left(t \right)} +\\ g l_{g1} m_{1} \cos{\left({q_{0}}{\left(t \right)} + {q_{1}}{\left(t \right)} \right)} +  l_{0} l_{g1} m_{1} \sin{\left({q_{1}}{\left(t \right)} \right)} \left(\frac{d}{d t} {q_{0}}{\left(t \right)}\right)^{2} +\\ l_{0} l_{g1} m_{1} \cos{\left({q_{1}}{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} +  l_{g1}^{2} m_{1} \frac{d^{2}}{d t^{2}} {q_{0}}{\left(t \right)} + l_{g1}^{2} m_{1} \frac{d^{2}}{d t^{2}} {q_{1}}{\left(t \right)} ...(1-26)$$

以上を行列式の形にすると、

$$\begin{bmatrix}Q_0 \cr Q_1\end{bmatrix}=\\ \begin{bmatrix}I_0+m_0l_{g0}^2+I_1+m_1(l_0^2+l_{g1}^2+2l_0l_{g1}cos\theta_1)& I_1+m_1(l_{g1}^2+l_0l_{g1}cos\theta_1)\cr I_1+m_1(l_{g1}^2+l_0l_{g1}cos\theta_1)& I_1+m_1 l_{g1}^2\end{bmatrix}\begin{bmatrix}\ddot{\theta_0} \cr \ddot{\theta_1}\end{bmatrix}\\+\begin{bmatrix}d_0& 0\cr 0& d_1\end{bmatrix}\begin{bmatrix}\dot{\theta_0} \cr \dot{\theta_1}\end{bmatrix}\\+\begin{bmatrix}-m_1l_0l_{g1}(2\dot{\theta_0}\dot{\theta_1}+\dot{\theta_1}^2)sin\theta_1& m_0gl_{g0}cos(\theta_0)+m_1g(l_{0}cos\theta_0+l_{g1}cos(\theta_0+\theta_1))\cr m_1l_0l_{g1}\dot{\theta_0}^2sin\theta_1& m_1gl_{g1}cos(\theta_0+\theta_1)\end{bmatrix} \\...(1-27)$$

参考資料と一致しました。OK!! (雑記2.)

simplyfy式変形後は順番がめちゃくちゃなので綺麗に行列の形に直すのはかなり面倒です。気をつけてください。(雑記3.)

ここまでの参考

シミュレーション

動く図を作っていきます。

順運動学と逆運動学

第二リンクの先端位置(x_0,y_0)は

$$\begin{bmatrix}x_0 \cr y_0\end{bmatrix}=\begin{bmatrix}l_{0}cos\theta_0\cr l_{0}sin\theta_0\end{bmatrix} ...(2-1)$$

第二リンクの先端位置(x_1,y_1)は

$$\begin{bmatrix}x \cr y\end{bmatrix}=\begin{bmatrix}x_1 \cr y_1\end{bmatrix}=\begin{bmatrix}l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1)\cr l_{0}sin\theta_0+ l_{1}sin(\theta_0+\theta_1)\end{bmatrix}...(2-2)$$

これを解いていきます。移項して,

$$x-l_{0}cos\theta_0= l_{1}cos(\theta_0+\theta_1)...(2-3)$$

$$y-l_{0}sin\theta_0= l_{1}sin(\theta_0+\theta_1) ...(2-4)$$

どちらも両辺2乗し、2式を足し合わせます。

$$x^2+y^2+l^2_0-2l_0(xcos\theta_0+ysin\theta_0)=l^2_1...(2-5)$$

cos,sinに関してまとめると,

$$\sqrt{x^2+y^2}cos(\theta_0+\alpha)=\frac{x^2+y^2+l^2_0-l^2_1}{2l_0}...(2-6)$$

ただし,

$$tan( \alpha)=\frac{y}{x}...(2-7)$$

$$∴ \theta_0=-\alpha\pm cos^{-1}(\frac{x^2+y^2+l^2_0-l^2_1}{2l^2_0\sqrt{x^2+y^2}})...(2-8)$$

次にθ_1を求めます。

(2-3)/(2-4)より,

$$\frac{y-l_{0}sin\theta_0}{x-l_{0}cos\theta_0}=tan(\theta_0+\theta_1)...(2-9)$$

$$∴\theta_1=-\theta_0+tan^{-1}(\frac{y-l_{0}sin\theta_0}{x-l_{0}cos\theta_0})...(2-10)$$

  • 参考

2-1.2リンクマニピュレータの軌道追従制御 -qiita

2-2.【Python】2リンクマニピュレータの逆運動学シミュレーション -西住工房

2-3.大分大学工学部福祉環境工学科メカトロニクスコース松尾研究室ゼミ資料"MATLAB による 2 リンクロボットマニピュレータ制御のシミュレーション"

順逆運動学計算用コード

  • そも
%matplotlib inline 
%matplotlib notebook

って何ですか、ということ。jupyter内で図を見えてくれるinlineは良く使います。使わなくても出るような気がするけど、出ない時にはとりあえず書き加える感じ。

一方notebookは編集可能らしい。アイコンをタッチしてから右クリックでドラッグしたり、いろんな操作ができ図を弄れる、上の電源ボタンみたいなもので画像を固定できる。

=>参考2-1、2-2

  • wedget

jupyter widget

公式のUsing~を見てみます。

%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np

@interact(amp=(0.1, 4.0, 0.1), omega=(0.1, 4.0, 0.1), phase=(-np.pi, np.pi, 0.1), 
          fn = {'sin': np.sin, 'cos': np.cos, 'tan': np.tan})
def h(amp=1.0, omega=1.0, phase=0.0, fn=np.sin):
    domain=[-np.pi, np.pi]
    x = np.linspace(domain[0], domain[1], 100)
    y  = amp * fn(omega * x + phase)
    plt.plot(x, y)
    plt.plot(-phase/omega, 0, 'or')
    plt.xlim(domain)
    plt.ylim([-4, 4])

x=(最小,最大,ステップ)はinteractでも設定でき、初期設定としてinteractを使用したい場合はデコレータとして使用出来る。辞書型で与えると選択肢に出来る。

  • コード 2

https://github.com/inomatly/robot_arm/blob/master/arm_sim.ipynb

一応.pyも置いてありますが jupyter notebok で見ることを前提にしています。

動いている様子:

楽しい。でもめっちゃ重い。(雑記3.)

このとき、(2-9)の±によって解(すなわち姿勢)は2通りあります。たとえば(1,1)は間接が(1,0)にある場合と(0,1)にある場合が考えられます。今回は計算を簡単にする為に1通りに統一しています。

特異点に関して

ヤコビ行列=0になるような姿勢は特異姿勢と呼ばれ、その点は特異点と呼ばれる。

(1)式を微分するとヤコビ行列Jは

$$\begin{bmatrix}\dot{x} \cr \dot{y}\end{bmatrix}=\begin{bmatrix}-l_{0}sin\theta_0 - l_{1}sin(\theta_0+\theta_1) & - l_{1}sin(\theta_0+\theta_1) \cr l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1) & l_{1}cos(\theta_0+\theta_1)\end{bmatrix}\begin{bmatrix}\dot{\theta_0} \cr \dot{\theta_1}\end{bmatrix}...(1)$$

$$∴ J=\begin{bmatrix}-l_{0}sin\theta_0 - l_{1}sin(\theta_0+\theta_1) & - l_{1}sin(\theta_0+\theta_1) \cr l_{0}cos\theta_0+ l_{1}cos(\theta_0+\theta_1) & l_{1}cos(\theta_0+\theta_1)\end{bmatrix}$$

特異点では

$$det J = l_0l_1sin\theta_1=0$$

$$∴ \theta_1=n\pi$$

n=0,1,2。この場合、たとえば(x,y) = (-2/√2,-2/√2)の伸びきった姿勢のような姿勢や、(0,0)のような縮こまった姿勢を示します。それ以上伸びたり縮んだり出来ない出来ませんから、一般に特異点では自由度が下がってることになります。

目標点が特異点を通る場合は問題ありませんが、特異点付近を通るときはアームが急激に動くため注意が必要です。(雑記5.)=>2-3

参考 2

2.1 Jupyter Notebookにおけるmatplotlib -Pythonオンライン学習サービスPyQ(パイキュー)

2-2 jupyter notebookにmatplotlibを使ってグラフを描画する -山本隆の開発日誌

2-3「ロボット工学 ー機械システムのベクトル解析ー, 広瀬茂男著 裳華房 2003年」の10章

雑記

  1. コードに関して、「pythonじゃなくてmatlabとかmathematicaでいいんじゃないの」とも思いますが。新しく覚えるおが面倒なのと、学生のうちは安価ですが将来的には金がかかりますのでなるべくオープンソースを使っていきたいという意思表示です。オープンソース贔屓です。判官贔屓的な。
  2. 「まだ微分で消耗してるの?」正月休みの1/3に手でゴリゴリ計算して、参考にしたのが正しいっぽいことを確認してからコード書いたので二度手間ですが、今後はラグランジュ方程式を使いまわせるのでOKということにしました。TEXコードまで出してくれるの神ですね。禁断の果実という感じがします。
  3. ラグランジュ方程式は使えるようになったかな(わからないけど)と思いますが、お恥ずかしながらベクトル的な解法に苦手意識があります。これからやっていきたいと思います。
  4. シミュレーションは、jupyter上の位置の問題なのか、かくかくしますね。難しい。動かしているときめっちゃチカチカするのは何故なのか。計算が重いんだろうか?だれかプログラミングおしえて。
  5. ひよこの真似やワクワクっ!な動作のように脇を開閉する動きはこの類い...ではないですね。人間の腕は自由度のバケモノですから。ワクワクの時の手首が特異点かと言えばそんなわけない。

「pythonによる制御工学入門」を読ませて頂きました。面白かったのでご紹介します。
pythonによる制御工学入門

pythonによる制御工学入門」を読ませて頂きました。面白かったのでご紹介します。

サポートページ =>『Pythonによる制御工学入門』サポートページ

期間:12日間程度(1日2~3時間)

特徴

内容

  • pythonの基礎
  • 制御モデル
    • 伝達関数モデル
    • 状態空間モデル
  • 制御対称のふるまい
    • 時間応答
    • 周波数応答
  • 制御系設計
    • PID制御
    • 2自由度制御
    • ゲインチューニング(限界感度法)
    • ゲインチューニング(モデルマッチング)
    • 状態フィードバック制御(極配置法)
    • 状態フィードバック制御(最適レギュレータ)
  • 安定性
    • 閉ループ
    • 開ループ
  • アドバンスドな制御系設計
    • オブザーバ
    • ロバスト制御

感想

私の事前知識:

古典制御の伝達関数やPID制御は授業で出てきたので何となくイメージ出来ていました(わかるとは言ってない)。現代制御は全くわかりませんでした。

pythonに関してはゆるふわで常用しています。

全体の感想

表題からわかるようにpythonですぐ動かせます。パラメータを弄って理解を深めたり、自分で設計するうえで真似すればに使えるため便利です。

実用や、制御を勉強するための入門として全体を俯瞰する目的で書かれていると思われます。そのため、後半は特にあっさりした説明になっています。

pythonの記法に関しては、(私は読み飛ばしましたがパッと見た感じ)必要な部分は書いてありそうです。pythonを使ったことのない方には多少大変かもしれません。

モジュールはNumpy,Matplotli,Scipy,Sympy,Python-control(, slycotもインストールする必要あり)。 Python-control (リンクは2019/12/25時点で最新バージョンのマニュアルへ)はMATLABっぽく書けるパッケージなので、MATLABを使ったことがある方に良いかもしれません。

モデル

最適レギュレータを下で紹介していますので、状態空間モデルに関して少し書きます。

伝達関数モデルではフィードバック系全体について、出力y(t)、入力u(t)に対してP(t)を

$$\frac{y}{u}=P(t)$$

の形で表します。それに対して状態空間モデルでは、内部の状態を全て観測して、

$$\dot{ \textbf{x}}(t)= \textbf{Ax}(t)+ \textbf{Bu}(t)$$ $$\textbf{y}(t)= \textbf{Cx}(t)+ \textbf{Du}(t)$$

の形で表します。例えば、このようなアームを考えてみます。

アームのモデル

トルクを$$\tau$$

粘性摩擦による力を$$\mu \dot{\theta}$$

重力加速度をgとすると、運動方程式は

$$\tau(t)=J\ddot{\theta}+\mu \dot{\theta}+Mglsin\theta$$

と書けます。入力u(t)、出力y(t)をそれぞれ

$$u(t)=\tau(t)$$

$$y(t)=\theta(t)$$

とし、角度 $\theta$ を十分小さいとして $sin(\theta)$ を

$$sin(y(t))=y(t)$$

と近似すると、

$$u(t)=J\ddot{y}(t)+\mu \dot{y}(t)+Mgly(t)$$

と書けます。これをもとに伝達関数が作れます。

さらに、状態空間モデルでは、

$$\textbf{x}(t)= \begin{bmatrix} \theta & \dot{\theta} \end{bmatrix}^T$$

と置いて

$$\dot{\textbf{x}}(t)= \begin{bmatrix} \dot{\theta} \cr \ddot{\theta} \end{bmatrix} =\begin{bmatrix} 0 & 1\cr -\frac{Mgl}{J} & -\frac{\mu}{J} \end{bmatrix}\textbf{x}(t) + \begin{bmatrix} 0 \cr \frac{1}{J} \end{bmatrix}u(t) $$

$$y(t)=\theta(t)=\begin{bmatrix} 1 & 0 \end{bmatrix} \textbf{x}(t) $$

となります。状態空間モデルの形にまとめると、

$$\dot{ \textbf{x}}(t)= \textbf{Ax}(t)+ \textbf{Bu}(t)$$ $$\textbf{y}(t)= \textbf{Cx}(t)+ \textbf{Du}(t)$$

$$\textbf{A}=\begin{bmatrix} 0 & 1\cr -\frac{Mgl}{J} & -\frac{\mu}{J} \end{bmatrix},\textbf{B}=\begin{bmatrix} 0 \cr \frac{1}{J} \end{bmatrix}, \textbf{C}=\begin{bmatrix} 1 & 0 \end{bmatrix},\textbf{D}=0$$

のように表せます。

ただし、状態空間モデルでは各パラメータが観測出来る必要があります。

序盤はかなりわかりやすく書かれています。古典制御と現代制御の基本的な部分はわかった気がします。

制御系設計

pythonで自分で動かせるという点で最もよかったのが設計の部分でした。教科書のグラフをサラッと見るよりわかった感がありました。

授業で使った教科書ではPID制御の理論をやったあとで、「なんで制御できるのかはわかったけど、kp,ki,kdの具体的な調べ方がわからん」と思ってたので、ゲインチューニングがコードと一緒に書いてあるのはかなり有り難かったです。

MATLABのようにマクローリン展開などが一瞬で出来ます。

(ここでちょっと思ったのが、MATLABのような有料ツールに近いことが無料で出来るのだから計算速度等が落ちているのではということ。実際のところはどうなのかは調べてません。)

個人的ハイライト:最適レギュレータ

p.176~状態空間モデルでのフィードバックゲインFを下のように与えます。

$$u=\textbf{Fx}$$

この F を"いい感じ"(雑)に選ぶ為に使われる手法の1つが最適レギュレータらしいです。

最適レギュレータでは、最適な値を求めるために、評価関数を最小にすることを考えます。

$$\textbf{Q}=\textbf{Q}^T>0,\textbf{R}=\textbf{R}^T>0$$

に対して評価関数

$$J=\int^\infty_0\textbf{x}(t)^T\textbf{Qx}(t)+u^T(t)\textbf{R}u(t)dt$$

を最小化するコントローラは

$$u=\textbf{F}_{opt}\textbf{x}$$

$$\textbf{F}_{opt}=\textbf{R}^{-1}\textbf{B}^T\textbf{P}$$

の形で得られます。ただし、

$$\textbf{P}=\textbf{P}^T>0$$

はリカッチ方程式

$$\textbf{A}^T\textbf{P}+\textbf{A}\textbf{P}+\textbf{P}\textbf{B}\textbf{R}^{-1}\textbf{B}\textbf{P}+\textbf{Q}=0$$

を満たす唯一の正定対象行列です。 なんでこうなるのかは詳しくは書いてませんが今後調べます。 また、Jの最小値は

$$\textbf{x}(0)^T\textbf{Px}(0)$$

です。

python上でMATLABの関数に似たものを使用できるpython_controlというものがあります。これで、一見難しそうな計算が一瞬の内に出来ます!!これはやばい。

import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline 
from control.matlab import *
# グラフの設定
def plot_set(fig_ax,*args):
    # x軸のラベルを1つ目を引数に
    fig_ax.set_xlabel(args[0])
    # y軸のラベルを2つ目を引数に
    fig_ax.set_ylabel(args[1])
    fig_ax.grid(ls=':')
    if len(args)==3:
        fig_ax.legend(loc=args[2])

A = [[0,1],[-4,-5]]
B = [[0],[1]]
C= np.eye(2)
D = np.zeros([2,1])
P= ss(A,B,C,D)#状態空間モデル
#最適レギュレータによるFの計算
Q = [[100,0],[0,1]]
R=1
X,E,F=care(P.A, P.B, Q, R)#フィードバックゲインF,リカッチ方程式の解X,閉ループ極Eを返す。
F=-F
#状態空間モデルでの応答
Acl = P.A+P.B*F
Pfb = ss(Acl, P.B, P.C, P.D)#状態空間モデル
Td = np.arange(0, 5, 0.01)#時間
Ud = 1*(Td>0)#時間Tdが正の範囲では1,負では0となるステップ信号。
X0 = [-0.3,0.4]#初期値
xst,t=step(Pfb,Td)#ステップ応答
xin,_=initial(Pfb,Td,X0)
x,_,_=lsim(Pfb,Ud,Td,X0)

fig, ax = plt.subplots(1,2,figsize=(6,2.3))
for i in [0,1]:
    ax[i].plot(t,x[:,i])
    ax[i].plot(t,xst[:,i], ls='-')
    ax[i].plot(t,xin[:,i], ls='-.')

plot_set(ax[0],'t','$x_1$')
plot_set(ax[1],'t','$x_2$')
plt.savefig('最適レギュレータによるフィードバック制御.png')
最適レギュレータのステップ応答。
( 青が応答、オレンジがゼロ状態応答、点線がゼロ入力応答。)

最適レギュレータの関数:

現代文明に感動しました。伝達関数モデルでのゲインチューニングの方法もすぐできて衝撃的でした。

次に読みたい

最後に参考文献としておすすめ本が載っているのもポイント高い。

短期集中:振動論と制御理論 工学系の数学入門 の感想です。

振動を例にモデル化しグラフを描くまで、制御の一番はじめの体験ができる本。

短期集中:振動論と制御理論 工学系の数学入門 日本評論社 吉田 勝俊

期間: ~2019年2月11日の3週間程度

内容(個人的まとめ)

  1. 力学の運動方程式
  2. 単振動,減衰振動,強制振動
  3. スケール変換
  4. ハーモニックバランス
  5. 複素数,ラプラス変換,伝達関数
  6. 解析力学入門
  7. 棒立て制御(倒立振子)
  8. 線形化

特徴

単振り子や倒立振子を取り上げて、ソフト「MATX」で計算を体験しながら理解できる本でした。全体的に簡単に書いているので、なんとなく理解できます。 キーワードが載っているので、次につなげることができます。

感想

(参考)読む前: 電気科3年後期のテスト期間でした。
力学は入試+学部1年でやったきりで、さっぱりわかりませんでした。 制御工学の初歩的知識は持っており、なんとなく伝達関数を記述できました。 また、私はpythonで計算しましたが、pythonはこれで初めてさわりました(最下部のリンク)。

全体

この本の内容は

うちの卒研生が配属直後の1カ月以内に本書の全課題をこなし、

本書 「はじめに」より

と書かれている通りの難易度です。

力学、振動の運動方程式の解き方など、非常に基本的な部分からスタートして倒立振子の姿勢制御までを紹介しています。 数値計算ソフトMATXを使用しながら演習できます。

私は MATX ではなくPythonでやりました。pythonは方程式を解いたりグラフを描いたりすることが簡単にできるのでおすすめです。MATXのコードが載っているので、コーディングの参考にできました。

力学がさっぱりわからない状態で、倒立振子の制御がやりたかった私に最適な本でした。参考にした本、この後に読むといい本が載っているのもかなり有難かったです。

1. 力学の運動方程式

よくある内容です。

2. 単振動,減衰振動,強制振動

電気科の私にはLRC回路が馴染みがありましたが、ばねとダンパーで復習できました。

3. スケール変換

2次方程式を解くのは面倒なので、パラメータを減らして1次方程式にします。このような操作をスケール変換と呼びます。 1次方程式にすると、プログラムで解くのが簡単になります。

4. ハーモニックバランス法

解き方です。

5. 複素数,ラプラス変換,伝達関数

基本的な内容です。

6. 解析力学入門

この本で、解析力学というものを初めて知りました。それだけでもこの本を読んで良かったと思いました。

解析力学では、運動の座標を変化させたり一般化して表現したりするみたいです。ラグランジュの運動方程式の知識があると、一般化された座標の運動方程式が比較的簡単に立てられます。

ラグランジュの運動方程式をこの本だけで理解するのは厳しかったです。

7. 棒立て制御(倒立振子)

ラグランジュの運動方程式は使わずに倒立振子の運動方程式を立てて、グラフに書きました(下記の関連記事)。途中式は(よくあることですが)書かれていないので大変でした。それっぽい波形が得られたので満足です。

8. 線形化

あまりよくわからなかったのでさらっと読みました。

次に読みたい

解析力学キャンパス・ゼミ 改訂2 マセマ出版社 馬場 敬之 (著)

力学 裳華房 原島 鮮 (著)

Pythonによる制御工学入門 オーム社 南 裕樹 (著)

関連記事

Jupyter Notebookで倒立振子の振動波形を表示する