状態フィードバック制御

状態フィードバック制御とは

システム $$ \dot{x} = x + u $$ について考える。このシステムの極は、特性方程式により、

$$ (s-1) = 0 $$ $s=1$が極となる。
したがって、このシステムは不安定である。

$$ x = A \exp{(st)} $$

ここで入力$u$を工夫して

$$ u = kx + v $$

このようにすると、

新たなシステムが

$$ \begin{align} \dot{x} &= x + kx + v \\ &= (1+k)x + v \end{align} $$

この特性方程式

$$ (s-(1+k)) = 0 $$ により $$ s = 1+k $$ となり、$s$を任意の値に設定することが出来る。

任意極配置

$$ \left\{ \begin{array}{c} \dot{\boldsymbol{x}} &=& A\boldsymbol{x}+\boldsymbol{b}u \\ y &=& \boldsymbol{cx} \end{array} \right. $$

これについて状態フィードバック
$$ u = \boldsymbol{kx} + v $$ を行う。ここで$\boldsymbol{k} = [k_1 \, k_2 \, \cdots \, k_n]$である。この$\boldsymbol{k}$をフィードバックゲインと呼ぶ。

これよりシステム全体は

$$ \begin{array}{l} \dot{\boldsymbol{x}} &=& A\boldsymbol{x}+\boldsymbol{b}u \\ &=& A\boldsymbol{x}+\boldsymbol{b}(\boldsymbol{kx}+v) \\ &=& (A+\boldsymbol{kb})\boldsymbol{x}+\boldsymbol{b}v \\ \end{array} $$

ここで新たにシステム行列$A$を $$ A_k = A+\boldsymbol{kb} $$ とする。システムの極は、システム行列$A_k$の固有値によって決定する。
$A_k$の特性方程式は $$ |sI-A_k| $$ となる。

任意極配置の設計法

希望する極 $\lambda_1,\, \lambda_2,\, \cdots ,\lambda_n$に配置するような状態フィードバック制御則

$$ u = \boldsymbol{kx} + v $$

を求めたい。 そのために、システムを可制御正準形

$$ \left\{ \begin{array}{l} \dot{\tilde{\boldsymbol{x}}} &=& A_c \tilde{\boldsymbol{x}}+\boldsymbol{b_c}u \\ y &=& \boldsymbol{c_c} \tilde{\boldsymbol{x}} \end{array} \right. $$

$$ A_c = \left[ \begin{array}{ccccc} 0 & 1 & 0 & \cdots & 0 \\ 0 & 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 1 \\ -a_0 & -a_1 & -a_2 & \cdots & -a_{n-1} \end{array} \right] , \boldsymbol{b_c} = \left[ \begin{array}{c} 0 \\ 0 \\ \vdots \\ 0 \\ 1 \end{array} \right] $$

$$ \boldsymbol{c_c} = \left[ \begin{array}{cccc} c_0 \, c_1 \, \cdots \, c_{n-1} \end{array} \right] $$

に変換する

例題1

次のシステムを考える

$$ A = \left[ \begin{array}{cc} 1 & 0 \\ 0 & 2 \end{array} \right] $$

import control as pc
import numpy as np

State = pc.ss([[1, 0],
               [0, 2]],
              [[1], 
               [1]],
              [[1, 1]], 0)

Mc = pc.ctrb(State.A, State.B)
print np.linalg.matrix_rank(Mc)
print Mc

2
[[1 1]
 [1 2]]

よって可制御である

State_canon, T = pc.canonical_form(State, "reachable")
State_canon

A = [[ 3. -2.]
 [ 1.  0.]]

B = [[ 1.]
 [ 0.]]

C = [[ 2. -3.]]

D = [[0]]

閉ループ系の特性多項式を求める

k = pc.place(State.A, State.B, [-2, -3])
k

array([[-12.,  20.]])

Ak = State.A - State.B*k
np.linalg.eig(Ak)

(array([-2., -3.]), matrix([[ 0.8       ,  0.78086881],
         [ 0.6       ,  0.62469505]]))

T

matrix([[-1.,  2.],
        [-1.,  1.]])

L = np.linalg.inv(T*Mc)
L

matrix([[ 1., -3.],
        [ 0.,  1.]])

Python-Controlでの話

Python-Controlモジュールでは少々勝手が変わってくるのでメモしておく

import control as pc
import numpy as np
sys = pc.ss([[1, 0],
             [0, 2]],
            [[1],[1]],
            [[1, 1]],
              0)

可制御行列$M_c$

$M_c$を求めるにはpc.ctrbを使う
$M_c$は

$$ M_{c} = \left[ B \ AB \ A^{2}B \ \cdots \ A^{n-1}B \right] $$

と定義される。

今回は $$ M_c = [A \, AB] = \left[ \begin{array}{cc} 1 & 1 \\ 1 & 2 \end{array} \right] $$ となる

Mc = pc.ctrb(sys.A, sys.B)
Mc

matrix([[1, 1],
        [1, 2]])

可制御正準形

可制御正準形を求めるにはpc.canonical_formを使う
可制御正準形にするためには変換行列$T_c$を

$$ T_c = (M_c L)^{-1} $$

として求める。ここで$L$は

$$ L = \left[ \begin{array}{ccccc} 1 & a_{n-1} & \cdots & a_{2} & a_{1} \\ 0 & 1 & \cdots & a_{3} & a_{2} \\ \vdots & \vdots & \ddots & \dots & \vdots \\ 0 & 0 & \cdots & 1 & a_{n-1} \\ 0 & 0 & \cdots & 0 & 1 \end{array} \right] $$

である。

sys_c, Tc = pc.canonical_form(sys, "reachable")
sys_c, Tc

(A = [[ 3. -2.]
  [ 1.  0.]]

 B = [[ 1.]
  [ 0.]]

 C = [[ 2. -3.]]

 D = [[0]], matrix([[-1.,  2.],
         [-1.,  1.]]))

任意極配置の状態フィードバックゲイン$\boldsymbol{k}$

任意の可制御システム$(A, B)$の極を任意の位置に配置するにはpc.placeを用いる。

Python-Controlでは、
$$ u = -\boldsymbol{kx} + v $$ としたフィードバックでのゲインを求めるようになっている

k = pc.place(sys.A, sys.B, [-2, -3])
k

array([[-12.,  20.]])

確認してみる

np.linalg.eig(sys.A-sys.B*k)

(array([-2., -3.]), matrix([[ 0.8       ,  0.78086881],
         [ 0.6       ,  0.62469505]]))

固有値は極に対応するため、ちゃんと固有値が-2と-3になっている。

開ループ系と閉ループ系の単位ステップ応答の確認

開ループ系

import matplotlib.pyplot as plt
import seaborn as sns
%matplotlib inline
x0 = [1.0, 0.1]
out_op, t_op = pc.step(sys, X0=x0)

fig = plt.figure(figsize=(10, 8))
plt.plot(t_op, out)

閉ループ系

import matplotlib.pyplot as plt
import seaborn as sns
%matplotlib inline
sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)
out_fb, t_fb = pc.step(sys_fb, X0=x0)

fig = plt.figure(figsize=(10, 8))
plt.plot(t_fb, out_fb)

閉ループ系の様々な応答の確認

ステップ応答と初期条件応答

import matplotlib.pyplot as plt
import seaborn as sns
%matplotlib inline
sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)
out_fb, t_fb = pc.step(sys_fb, X0=x0)
fig = plt.figure(figsize=(10, 8))
plt.plot(t_fb, out_fb, label="step")

sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)
out_fb, t_fb = pc.initial(sys_fb, X0=x0)
plt.plot(t_fb, out_fb, label="initial")

plt.legend()

インパルス応答

fig = plt.figure(figsize=(10, 8))
sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)
out_fb, t_fb = pc.impulse(sys_fb)
plt.plot(t_fb, out_fb)

線形システム応答

おそらくこれが最も汎用的な応答である。
pc.lsim(sys, U, T, X0)という使い方をする。

sysは状態システム
Uは入力の配列 Tは時間の配列
X0は初期条件である

もちろんそれぞれで、UとTは同次元の配列でなければならない X0は,sysで定義されているsys.Aの行と列の次元に一致しなければならない、などの制約がある。

自由応答(U=0.0)

fig = plt.figure(figsize=(10, 8))

sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)

x0 = [1.0, 0.1]
N = 1024
t = np.linspace(0, 4, 1024)
u = np.zeros(N)
out_fb, t_fb, x_fb = pc.lsim(sys_fb, U=u, T=t, X0=x0)
plt.plot(t_fb, out_fb, label="output")
plt.plot(t_fb, x_fb[:,0], label="$X_1$")
plt.plot(t_fb, x_fb[:,1], label="$X_2$")
plt.legend()

状態変数$x_0, x_1$ともに0に収束し、出力も目標値である入力$u$に収束して0になっている。

ステップ応答

fig = plt.figure(figsize=(10, 8))

sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)

x0 = [1.0, 0.1]
N = 1024
t = np.linspace(0, 4, 1024)
u = np.around(np.linspace(0, 1, N))
out_fb, t_fb, x_fb = pc.lsim(sys_fb, U=u, T=t, X0=x0)
plt.plot(t_fb, out_fb, label="Output")
plt.plot(t_fb, x_fb[:,0], label="$X_1$")
plt.plot(t_fb, x_fb[:,1], label="$X_2$")
plt.plot(t_fb, u, label="Input")
plt.legend()

矩形波(初期条件 [1.0, 0.1])

fig = plt.figure(figsize=(10, 8))

sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)

x0 = [1.0, 0.1]
N = 1024
t = np.linspace(0, 4, 1024)
u = np.sign(np.sign(np.sin(2*np.pi*t)))
out_fb, t_fb, x_fb = pc.lsim(sys_fb, U=u, T=t, X0=x0)
plt.plot(t_fb, out_fb, label="Output")
plt.plot(t_fb, x_fb[:,0], label="$X_1$")
plt.plot(t_fb, x_fb[:,1], label="$X_2$")
plt.plot(t_fb, u, label="Input")
plt.legend()

矩形波(初期条件 [0.0, 0.0])

fig = plt.figure(figsize=(10, 8))

sys_fb = pc.ss(sys.A-sys.B*k, sys.B, sys.C, sys.D)

x0 = [0.0, 0.0]
N = 1024
f = 1
t = np.linspace(0, 4, N)
u = np.sign(np.sin(2*np.pi*f*t))
out_fb, t_fb, x_fb = pc.lsim(sys_fb, U=u, T=t, X0=x0)
plt.plot(t_fb, out_fb, label="Output")
plt.plot(t_fb, x_fb[:,0], label="$X_1$")
plt.plot(t_fb, x_fb[:,1], label="$X_2$")
plt.plot(t_fb, u, label="Input")
plt.legend()