Python で見る現代制御
numpy, scipy.integrate.odeint, matplotlib.pyplot を使うため、まず
import する。
>>> import numpy
>>> import matplotlib.pyplot as plt
>>> from scipy.integrate import odeint
□ 可制御性・可観測性
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-2 , -1; -1, -2], b = [1; 1], c = [1, 0]
の可制御性と，可観測性を調べる．
>>> A = numpy.array([[-2, -1],[-1, -2]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> Mc = numpy.column_stack((b, A.dot(b)))
>>> numpy.linalg.det(Mc)
Mc の行列式が 0 なので可制御ではない．
>>> Mo = numpy.row_stack((c, c.dot(A)))
>>> numpy.linalg.det(Mo)
Mo の行列式が 0 でないので可観測である．
□ 対角化
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-2 , -1; -1, -2], b = [1; 1], c = [1, 0]
を対角化する．
>>> A = numpy.array([[-2, -1],[-1, -2]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> (V, T) = numpy.linalg.eig(A)
>>> Abar = numpy.linalg.inv(T).dot(A).dot(T)
>>> bbar = numpy.linalg.inv(T).dot(b)
>>> cbar = c.dot(T)
>>> print Abar, bbar, cbar
この結果，システムが可制御ではないが可観測であることがわかる．
□ 可制御正準形
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1 , 2; -1, -4], b = [1; 1], c = [1, 0]
を可制御正準形に変換する．
>>> A = numpy.array([[-1, 2], [-1, -4]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> p = numpy.poly(A)
>>> p
特性多項式が s^2 + 5 s + 6 となることがわかる．
>>> S = numpy.array([[p[1], 1],[1, 0]])
>>> T = numpy.column_stack((b, A.dot(b))).dot(S)
>>> iT = numpy.linalg.inv(T)
>>> Abar = iT.dot(A).dot(T)
>>> bbar = iT.dot(b)
>>> cbar = c.dot(T)
>>> print Abar, bbar, cbar
□ 安定性判別
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1 , 2; -1, -4], b = [1; 1], c = [1, 0]
の安定性を判別する．
>>> A = numpy.array([[-1, 2], [-1, -4]])
>>> numpy.roots(numpy.poly(A))
A の特性方程式の根の実部が全て負なので，システムは安定である．
□ 状態フィードバック制御の極配置
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1 , -2; -1, -4], b = [1; 1], c = [1, 0]
の極を -1+2j, -1-2j に設定する状態フィードバックゲイン f を求める．
>>> A = numpy.array([[-1, -2], [-1, -4]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> p1 = numpy.poly(A)
>>> S = numpy.array([[p[1], 1],[1, 0]])
>>> T = numpy.column_stack((b, A.dot(b))).dot(S)
>>> iT = numpy.linalg.inv(T)
>>> p2 = numpy.poly(numpy.array([-1+2j, -1-2j]))
>>> f = numpy.array([p2[2] - p1[2], p2[1] - p1[1]]).dot(iT)
>>> f
初期値 x(0) = [1; -1] の場合の応答を確かめる．
>>> global A,b,f
>>> def F(x, t):
global A, b, f
return (A-numpy.outer(b,f)).dot(x)
>>> x0 = numpy.array([1, -1])
>>> t = numpy.linspace(0, 5, 1001)
>>> y = odeint(F, x0, t)
>>> plt.plot(t, y)
>>> plt.grid('on')
>>> plt.show()
□ 直接フィードバック制御における根軌跡
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1, 0, 0; -1, -1, -1; -2, 1, -2], b = [1; 1; 1], c = [0, 0, 1]
に対して u(t) = -k y(t) という直接フィードバックを行ったときの根軌跡を
求める．
>>> A = numpy.array([[-1, 0, 0], [-1, -1, -1], [-2, 1, -2]])
>>> b = numpy.array([1, 1, 1])
>>> c = numpy.array([0, 0, 1])
>>> global A, b, c
>>> def F(k):
global A, b, c
return numpy.roots(numpy.poly(A- k*numpy.outer(b,c)))
>>> k = numpy.linspace(0, 30, 61)
>>> nc = numpy.size(k)
>>> r = F(k[0])
>>> for i in range(nc)[1:]:
r = numpy.concatenate((r, F(k[i])))
>>> plt.plot(numpy.real(r), numpy.imag(r), 'x')
>>> plt.grid('on')
>>> plt.show()
□ 同一次元オブザーバの構成
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1 , -2; -1, -4], b = [1; 1], c = [1, 0]
に対して，-4, -5 に極を持つ同一次元オブザーバ
z'(t) = A z(t) -k[c z(t) - y(t)] + b u(t) = (A - k c)z(t) + k y(t) + b u(t)
のゲイン k を決定する．
>>> A = numpy.array([[-1, -2], [-1, -4]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> p1 = numpy.poly(A)
>>> S = numpy.array([[p[1], 1],[1, 0]])
>>> T = numpy.column_stack((c, A.T.dot(c))).dot(S)
>>> iT = numpy.linalg.inv(T)
>>> p2 = numpy.poly([-4, -5])
>>> k = numpy.array([p2[2] - p1[2], p2[1] - p1[1]]).dot(iT)
>>> k
オブザーバの推定の様子を確かめる．初期値 x(0) = [0; 0], z(0) = [1; 1]
u(t) = 1 (単位ステップ入力)とする．
>>> A1 = numpy.column_stack((A, numpy.zeros((2,2))))
>>> A2 = numpy.column_stack((numpy.outer(k,c),\
A-numpy.outer(k,c)))
>>> AA = numpy.row_stack((A1, A2))
>>> bb = numpy.concatenate((b, b))
>>> global AA, bb
>>> def F(x, t):
global AA, bb
return AA.dot(x) + bb
>>> x0 = numpy.array([0, 0, 1, 1])
>>> t = numpy.linspace(0, 5, 1001)
>>> y = odeint(F, x0, t)
>>> plt.plot(t, y)
>>> plt.grid('on')
>>> plt.show()
□ 同一次元オブザーバとフィードバック制御の併合システム
システム
x'(t) = A x(t) + b u(t)
y(t) = c x(t)
A = [-1 , -2; -1, -4], b = [1; 1], c = [1, 0]
に対して，-4, -5 に極を持つ同一次元オブザーバ
z'(t) = A z(t) -k[c z(t) - y(t)] + b u(t) = (A - k c)z(t) + k y(t) + b u(t)
の推定値を用いて，極を -1+2j, -1-2j に配置したフィードバック制御を
行う．
>>> A = numpy.array([[-1, -2], [-1, -4]])
>>> b = numpy.array([1, 1])
>>> c = numpy.array([1, 0])
>>> p1 = numpy.poly(A)
>>> S = numpy.array([[p[1], 1],[1, 0]])
>>> T = numpy.column_stack((c, A.T.dot(c))).dot(S)
>>> iT = numpy.linalg.inv(T)
>>> p2 = numpy.poly([-4, -5])
>>> k = numpy.array([p2[2] - p1[2], p2[1] - p1[1]]).dot(iT)
>>> T = numpy.column_stack((b, A.dot(b))).dot(S)
>>> iT = numpy.linalg.inv(T)
>>> p2 = numpy.poly([-1+2j, -1-2j])
>>> f = numpy.array([p2[2] - p1[2], p2[1] - p1[1]]).dot(iT)
>>> print k, f
併合システムの応答を確かめる．
初期値 x(0) = [1; -1], z(0) = [0; 0] とする．
>>> A1 = numpy.column_stack((A, -numpy.outer(b,f)))
>>> A2 = numpy.column_stack((numpy.outer(k,c),\
A - numpy.outer(k,c) - numpy.outer(b,f)))
>>> AA = numpy.row_stack((A1, A2))
>>> global AA
>>> def F(x, t):
global AA
return AA.dot(x)
>>> x0 = numpy.array([1, -1, 0, 0])
>>> t = numpy.linspace(0, 5, 1001)
>>> y = odeint(F, x0, t)
>>> plt.plot(t, y)
>>> plt.grid('on')
>>> plt.show()
Python で数値計算
naniwa@rbt.his.u-fukui.ac.jp