はじめに

今回は、前回同定したモデルを用いて、アーム角度をサーボ制御をします。具体的には線形二次積分コントローラ(LQI)と同一次元オブザーバ、定常カルマンフィルタを組み合わせた出力フィードバックコントローラの設計、評価します。また、私が制御工学を学んだ際、コントローラ設計はわかったし、効果もわかったけど、どうやってマイコンとかに実装するの？と疑問に思っていたので、今回は実際に設計したコントローラをマイコンに実装するやり方も簡単に説明できればと思います。

前回の記事のリンクは下記になります。

目次

1. 実験装置
 2. 水平1軸アームシステムのモデル
 3. 制御系設計
 4. シミュレーション
 5. 実機実験
 まとめ
 参考文献

1. 実験装置

実験装置を図1.1に示します。使用した主要部品を表1.1に示します。図1.1のようにモータとエンコーダは歯車でつながっており、減速比は${1:1}$になっています。またエンコーダは4逓倍で使用したので分解能としては${2.618 \times 10^{-3} \mathrm{[rad/pulse]}}$となっています。
電源電圧は${12 \mathrm{[V]}}$、入力するPWMの周波数は${20 \mathrm{[kHz]}}$、分解能を${2048}$、制御周期は${1 \mathrm{[ms]}}$としました。

表1.1. 実験装置の部品詳細

項目	詳細
DCモータ	DME34B37G30A (12V)
エンコーダ	E6B2-CWZ6C (600P/R)
マイコン	ESP32-DevKitC ESP32-WROOM-32開発ボード 4MB


図1.1. 実験装置

2. 水平1軸アームシステムのモデル

2.1. 非線形モデル

水平面を回転する1軸アームシステムの運動方程式は次のように表されます。このとき${\theta(t)}$をアームの角度変位${\mathrm{[rad]}}$、${J}$を慣性モーメント${\mathrm{[kg\cdot m^2]}}$、${C}$を粘性摩擦係数${\mathrm{[N\cdot m/(m/s)]}}$、${\tau(t)}$を入力トルク${\mathrm{[N\cdot m]}}$、${\tau_d}$を動摩擦トルク${\mathrm{[N\cdot m]}}$とします。

J\ddot{\theta}(t)+C\dot{\theta}(t)=\tau(t)-{\tau}_{d}(t)\mathrm{sgn}(\dot{\theta}(t)) \tag{2.1}

今回、実際にモータに入力しているのは電圧${V(t)}$ですので、式(2.1)を電圧入力に変更し、次のように整理します。このとき${i(t)}$を電流${\mathrm{[A]}}$、${K_T}$をトルク定数${\mathrm{[Nm/A]}}$、${R}$を内部抵抗${\mathrm{[\Omega]}}$とします。

\begin{align}
\ddot{\theta}(t) &= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{\tau(t)}{J}\\
&= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{K_T}{J} i(t)\\
&= -\frac{C}{J}\dot{\theta}(t) - \frac{\tau_{d}}{J} \mathrm{sgn} (\dot{\theta}(t)) + \frac{K_T}{JR} V(t) \tag{2.2}
\end{align}

このとき${\ddot{\theta}(t)}$、${\dot{\theta}(t)}$、${V(t)}$の各係数をそれぞれ${\alpha}$、${\beta}$、${\gamma}$とすると、式(2.2)は次のように表されます。

\begin{align}
\ddot{\theta}(t)=-\alpha \dot{\theta}(t)-\beta \mathrm{sgn} (\dot{\theta}(t))+\gamma V(t) \tag{2.3}
\end{align}

前回、予測誤差法(PEM)で同定したパラメータを表2.1に示します。今回は式(2.3)および下記パラメータを用いて、非線形モデルとして制御系の評価に使用します。


図2.1. 水平面を回転するアームシステム

表2.1. 同定したモデルのパラメータ

${\alpha}$	${\beta}$	${\gamma}$
25.6	16.3	39.4

2.2. 状態方程式

制御系設計をするのに状態方程式が必要なので、連続時間系の状態方程式を考えます。
状態変数${{\boldsymbol{x}(t)}}$、入力${{\boldsymbol{u}(t)}}$をそれぞれ次式のように定義します。

\boldsymbol{x}(t)=
\begin{bmatrix}
x_1(t) & x_2(t)
\end{bmatrix}
^T
=
\begin{bmatrix}
{\theta}(t) & \dot{\theta}(t)
\end{bmatrix}
^T
\tag{2.4}

\boldsymbol{u}(t)=V(t)
\tag{2.5}

式(2.3)、(2.4)、(2.5)より、${{\dot{\theta}}(t)=0 \mathrm{[rad/s]}}$のときを平衡状態と考え、平衡状態近傍で線形化をしたとき、${\dot{x}_1}$、${\dot{x}_2}$は次のように表されます。

\dot{x}_1=\dot{\theta}(t)=x_2(t)=0 \cdot x_1(t) + 1 \cdot x_2(t) + 0 \cdot u(t)
\tag{2.6}

\dot{x}_2=\ddot{\theta}(t)=-\alpha \dot{\theta}(t) + \gamma u(t) = 0 \cdot x_1(t) - \alpha x_2(t) + \gamma u(t)
\tag{2.7}

以上より、水平1軸アームシステムの状態方程式は次のように表されます。

\dot{\boldsymbol{x}}(t)
=\boldsymbol{Ax}(t)
+
\boldsymbol{Bu}(t)
\tag{2.8}

また、制御出力は${\boldsymbol{y}=[x_1(t)]}$なので、出力方程式は次のようになります。

\boldsymbol{y}
=
1 \cdot x_1(t) + 0 \cdot x_2(t) + 0 \cdot u(t)
=
\begin{bmatrix}
1 & 0
\end{bmatrix}
\begin{bmatrix}
x_1(t) \\
x_2(t)
\end{bmatrix}
+ 0 \cdot u(t)
=
\boldsymbol{Cx}(t)
\tag{2.9}

ただし、${\boldsymbol{A}}$、${\boldsymbol{B}}$、${\boldsymbol{C}}$ はそれぞれ次のようになります。

\boldsymbol{A}
=
\begin{bmatrix}
0 & 1\\
0 & -\alpha\\
\end{bmatrix}
,\quad
\boldsymbol{B}
=
\begin{bmatrix}
0\\
\gamma
\end{bmatrix}
,\quad
\boldsymbol{C}
=
\begin{bmatrix}
1 & 0
\end{bmatrix}

3. 制御系設計

この章では同定したモデルを用いて制御系設計、つまりコントローラ、オブザーバの設計をします。また実際に設計していく流れや、用語などについて説明します。

3.1. コントローラ

3.1.1. 可制御性

コントローラを設計する前に、対象システムに適切な入力を加えた際、システムの状態(角度、角速度など)を任意の状態にすることができるか、つまりシステムが制御可能かを確認する必要があります。こういったとき、システムが可制御かを判定すればいいです。可制御性の定義は下記のようになっています。

可制御性の定義

任意の初期状態のとき、有限時間内に制御入力を使ってシステムを望む任意の状態に遷移させることが可能なことを、システムが可制御であるという。

可制御性の判定は、システムの可制御行列${\boldsymbol{V}_c}$がフルランクかを確認すればよいです。システムの係数行列${\boldsymbol{A}}$、${\boldsymbol{B}}$、${\boldsymbol{C}}$がそれぞれ${n{\times}n}$、${n{\times}m}$、${r{\times}n}$の定数行列とします。このとき、可制御性行列${\boldsymbol{V}_c}$は次のように定義されます。

\boldsymbol{V}_c :=
\begin{bmatrix}
\boldsymbol{B} & \boldsymbol{AB} & \cdots & \boldsymbol{A}^{n-1}\boldsymbol{B}
\end{bmatrix}
\in
\mathbb{R}^{n{\times}mn}

\tag{3.1}

それでは本システムの可制御性の判別をしてみます。可制御性の判別は手計算でも求められますが、MatlabやPythonなど、プログラムで簡単に確認することができます。下記にPythonで可制御性を判定するコードを示します。この結果より、${\mathrm{rank}\boldsymbol{V}_c=2}$なので、本システムは可制御となります。

controllability.py

# 可制御性判別用関数
def ctrb(A, B):
    n = A.shape[0]
    controllability = B

    for i in range(1, n):
        controllability = np.hstack((controllability, np.linalg.matrix_power(A, i) @ B))
    return controllability

def is_controllable(A, B):
    ctr_matrix = ctrb(A, B)
    rank = np.linalg.matrix_rank(ctr_matrix)
    n = A.shape[0]
    print(f"Controllability Matrix:\n{ctr_matrix}")
    print(f"Rank of Controllability Matrix: {rank}")

出力結果

Controllability Matrix:
[[    0.      39.4 ]
 [   39.4  -1008.64]]
Rank of Controllability Matrix: 2

3.1.2. 線形二次積分コントローラ(LQI)の設計

現代制御では、状態フィードバック形式のコントローラがよく紹介されています。しかし、実際に制御する場合、レギュレータ制御ではなく、制御量を目標値に追従させるサーボ制御をしたい場面が多いです。今回は線形二次積分コントローラ(LQI)を最適レギュレータ理論を用いてサーボシステムを設計します。

目標軌道${\boldsymbol{x}_{ref}(t)}$と状態変数${\boldsymbol{x}(t)}$との偏差を式(3.2)、出力目標軌道と出力の偏差の積分を式(3.3)のように定義します。

\boldsymbol{x}_e(t)
=
\boldsymbol{x}_{ref}(t)-\boldsymbol{x}(t)
\tag{3.2}

\boldsymbol{w}(t)=\int_0^t ({\boldsymbol{Cx}_{ref}(t)-\boldsymbol{Cx}(t)})dt
\tag{3.3}

式(3.2)、(3.3)より、定義される拡大偏差システムの状態変数${\tilde{\boldsymbol{x}}_e(t)}$と入力${\boldsymbol{u}_e(t)}$は次のように表されます。

\boldsymbol{\tilde{x}}_e(t)
=
\begin{bmatrix}
\boldsymbol{{x}_e(t)}^T & \boldsymbol{w}(t)^T 
\end{bmatrix}
^T
\tag{3.4}

\boldsymbol{\tilde{u}}(t)=\boldsymbol{u}(t)-\boldsymbol{u}_{\infty}(t)
\tag{3.5}

式(2.8)、(2.9)、(3.4)、(3.5)より、拡大偏差システムの状態方程式は次のように表されます。

\begin{bmatrix}
{\boldsymbol{\dot{x}}_e(t)}\\
{\boldsymbol{\dot{w}}(t)}\\
\end{bmatrix}
=\boldsymbol{A}_e \boldsymbol{\tilde{x}}_e(t) + \boldsymbol{B}_e \boldsymbol{\tilde{u}}(t)
\tag{3.6}

\boldsymbol{e}(t)=\boldsymbol{C}_e \boldsymbol{\tilde{x}}_e(t)
\tag{3.7}

\boldsymbol{A}_e=
\begin{bmatrix}
\boldsymbol{A} & \boldsymbol{0}\\
-\boldsymbol{C} & \boldsymbol{0}\\
\end{bmatrix}
,\quad
\boldsymbol{B}_e=
\begin{bmatrix}
{\boldsymbol{B}}\\
{\boldsymbol{0}}\\
\end{bmatrix}
,\quad
\boldsymbol{C}_e
=
\begin{bmatrix}
-\boldsymbol{C} & \boldsymbol{0}
\end{bmatrix}

この拡大偏差システムが内部安定されるようなコントローラとして、線形二次積分コントローラ(LQI)を最適レギュレータ理論により設計します。最適レギュレータ理論では、次の二次形式評価関数を最小化するコントローラゲインを設計できます。ただし

\boldsymbol{Q}_{11}=\boldsymbol{Q}_{11}^T
,\quad
\boldsymbol{Q}_{22}=\boldsymbol{Q}_{22}^T
,\quad
\boldsymbol{Q}=\boldsymbol{Q}^T

は正定対称行列、${\boldsymbol{R}=\boldsymbol{R}^T}$は正の実数とします。また${\boldsymbol{K}}$は状態偏差${\boldsymbol{x}_e(t)}$に対するフィードバックゲイン、${\boldsymbol{G}}$は出力偏差の積分${\boldsymbol{w}(t)}$に対するフィードバックゲインとします。

J=\int_0^\infty (\boldsymbol{x}_e(t)^T \boldsymbol{Q}_{11} \boldsymbol{x}_e(t)+\boldsymbol{w}(t)^T \boldsymbol{Q}_{22} \boldsymbol{w}(t)+\boldsymbol{\tilde{u}}(t)^T \boldsymbol{R} \boldsymbol{\tilde{u}}(t)) dt

=\int_0^\infty (\boldsymbol{\tilde{x}}_e(t)^T \boldsymbol{Q} \boldsymbol{\tilde{x}}_e(t)+\boldsymbol{\tilde{u}}(t)^T \boldsymbol{R} \boldsymbol{\tilde{u}}(t)) dt
\tag{3.8}

\boldsymbol{\tilde{u}}(t)=\boldsymbol{K}_e \boldsymbol{\tilde{x}}_e (t)
,\quad
\boldsymbol{K}_e
=
\begin{bmatrix}
\boldsymbol{K} & \boldsymbol{G}
\end{bmatrix}
\tag{3.9}

コントローラゲイン${\boldsymbol{K}_e}$は、最適レギュレータ理論により、式(3.10)のリカッチ方程式を解くことで求まる正定対称解${\boldsymbol{P}}$を用いて式(3.11)のように唯一に定まります。

\boldsymbol{PA}_e+\boldsymbol{A}_e^T \boldsymbol{P}-\boldsymbol{PB}_e\boldsymbol{R}^{-1}+\boldsymbol{Q}=\boldsymbol{0}
\tag{3.10}

\boldsymbol{K}_e=
\begin{bmatrix}
\boldsymbol{K} & \boldsymbol{G}
\end{bmatrix}
=
-\boldsymbol{R}^{-1}\boldsymbol{B}_e^T\boldsymbol{P}
=
-\boldsymbol{R}^{-1}\boldsymbol{B}_e^T
\begin{bmatrix}
\boldsymbol{P}_{11} & \boldsymbol{P}_{12}\\
\boldsymbol{P}_{12}^T & \boldsymbol{P}_{22}
\end{bmatrix}

=
\begin{bmatrix}
-\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}_{11} & -\boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P}_{12}
\end{bmatrix}
\tag{3.11}

このとき、評価関数${J}$の最小値${J_{min}}$は状態の初期値${\boldsymbol{x}_0}$を用いて次のように表されます。

J_{min}=\boldsymbol{x}_0^T \boldsymbol{Px}_0
\tag{3.12}

以上より、システムに印加する入力${\boldsymbol{\tilde{u}}(t)}$は式(3.13)のようになります。

\boldsymbol{\tilde{u}}(t)=\boldsymbol{K}_e \boldsymbol{\tilde{x}}_e(t)-\boldsymbol{u}_{\infty}(t)
\tag{3.13}

ここで、${\boldsymbol{x}(t)=\boldsymbol{x}_{ref}(t)}$のとき、つまり目標軌道にシステムが追従している場合、入力${\boldsymbol{\tilde{u}}(t)}$は式(3.14)のように表されます。

\boldsymbol{\tilde{u}}(t)=\boldsymbol{K}_e \boldsymbol{\tilde{x}}_e(t)
\quad
(\because\boldsymbol{u}_{\infty}=0)
\tag{3.14}

長々と数式を書いていきましたが、実際にゲインを求めるのは簡単です。自分が重視してほしい状態変数の重みを大きくすることで、それに対応したゲインを求めることができます。
例えば、本システムの場合、${\boldsymbol{Q}}$ の ${(1,1)}$ 成分は、${\theta(t)}$、${(2,2)}$ 成分は ${\dot{\theta}(t)}$ 、${(3,3)}$ 成分は ${\boldsymbol{w}(t)}$ にあたります。そのため、定常偏差をなくしたい！ということなら ${(3,3)}$ 成分の値を大きくします。逆に ${\dot{\theta}(t)}$ はあまり気にしなくてもいいかなということなら ${(2,2)}$ 成分の値を小さくします。${\boldsymbol{R}}$ は入力の重みを表しており、値を大きくするほど、入力を小さくするようにゲインを決定、つまりゲインが小さくなります。逆に値を小さくすると、入力が大きくなるようゲインを決定、つまりゲインが大きくなります。
今回、${\boldsymbol{Q}}$、${\boldsymbol{R}}$はそれぞれ次式のように設定しました。

\boldsymbol{Q}={\rm{diag}}(1.0\times10^5 \quad 7.5\times10^2 \quad 3.0\times10^7),
\quad
\boldsymbol{R}=1.0\times10^0
\tag{3.15}

今回、ゲインは次のような値になりました。

\boldsymbol{K}=
\begin{bmatrix}
-6.376\times10^2 & -2.733\times10^1
\end{bmatrix}
,\quad
\boldsymbol{G}=5.477\times10^3
\tag{3.16}

下記にリカッチ方程式を解き、コントローラゲインを求めるプログラムを示します。

lqi_design.py

# 線形二次積分コントローラ設計
def lqi_design(A, B, C, Q, R):
    Ae = np.block([
        [A, np.zeros((A.shape[0], 1))],
        [-C, np.zeros((C.shape[0], 1))]
    ])
    Be = np.block([
        [B],
        [np.zeros((B.shape[1], 1))]
    ])
    Ce = np.block([
        [C, np.zeros((C.shape[0], 1))]
    ])

    P = solve_continuous_are(Ae, Be, Q, R)
    K = -np.linalg.inv(R).dot(Be.T).dot(P)

    print("Q : \r\n", Q)
    print("R : ", R)
    print("K : ", K)

    eig_ctrl_val = eigvals(Ae + Be @ K)
    print("eig_ctrl_val : \r\n", eig_ctrl_val)

    return K, eig_ctrl_val

出力結果

Q : 
 [[1.0e+05 0.0e+00 0.0e+00]
 [0.0e+00 7.5e+02 0.0e+00]
 [0.0e+00 0.0e+00 3.0e+07]]
R :  [[1.]]
K :  [[-637.56334791  -27.32856312 5477.22557505]]
eig_ctrl_val :
 [-1079.25535475+0.j           -11.54501603+8.16503366j
   -11.54501603-8.16503366j]

3.2. オブザーバ

式(2.8)、(2.9)より、本システムは状態変数が2つありますが、観測できる状態変数は1つのみです。コントローラはすべての状態変数が観測できる前提で設計しているため、今のままでは制御することができません。そこで、観測できる状態変数と入力から状態変数を推定するオブザーバを構築します。

3.2.1. 可観測性

オブザーバを設計する前に、対象システムに適切な入出力データから、システムの状態(角度、角速度など)を任意の速さで正確に推定することができるか、つまりシステムの状態を推定可能かを確認する必要があります。こういったとき、システムが可観測かを判定すればいいです。可観測性の定義は下記のようになっています。

可観測性の定義

任意の初期状態のとき、有限時間内にシステムの入力と出力の情報から、任意の状態を推定可能なことを、システムが可観測であるという。

可観測性の判定は、前述した可制御性の判定とほぼ同じで、システムの可観測行列${\boldsymbol{V}_o}$がフルランクかを確認すればよいです。システムの係数行列${\boldsymbol{A}}$、${\boldsymbol{B}}$、${\boldsymbol{C}}$がそれぞれ${n{\times}n}$、${n{\times}m}$、${r{\times}n}$の定数行列とします。このとき、可制御性行列${\boldsymbol{V}_o}$は次のように定義されます。

\boldsymbol{V}_o :=
\begin{bmatrix}
\boldsymbol{C}\\
\boldsymbol{CA}\\
\vdots\\
\boldsymbol{C}\boldsymbol{A}^{n-1}
\end{bmatrix}
\in
\mathbb{R}^{nr{\times}n}

\tag{3.17}

それでは本システムの可観測性の判別をしてみます。可制御性の判別同様、可観測性の判別もプログラムで簡単に確認することができます。下記にPythonで可観測性を判定するコードを示します。この結果より、${\mathrm{rank}\boldsymbol{V}_o=2}$なので、本システムは可観測となります。

controllability.py

# 可観測性判別用関数
def obsv(A, C):
    n = A.shape[0]
    observability = C

    for i in range(1, n):
        observability = np.vstack((observability, C @ np.linalg.matrix_power(A, i)))
    return observability

def is_observable(A, C):
    obs_matrix = obsv(A, C)
    rank = np.linalg.matrix_rank(obs_matrix)
    n = A.shape[0]
    print(f"Observability Matrix:\n{obs_matrix}")
    print(f"Rank of Observability Matrix: {rank}")

出力結果

Observability Matrix:
[[1. 0.]
 [0. 1.]]
Rank of Observability Matrix: 2

3.2.2. 同一次元オブザーバの設計

それではオブザーバについて考えていきます。今回、基本的なオブザーバの一つである同一次元オブザーバを設計します。これは状態変数${\boldsymbol{x}(t)}$と推定値${\hat{\boldsymbol{x}}(t)}$が同じ次元であることからそう呼ばれています。

制御対象の状態方程式は式(2.8)、(2.9)のように与えられたとき、状態変数${\boldsymbol{x}(t)}$の推定値${\hat{\boldsymbol{x}}(t)}$を推定することを考えます。このとき、推定値${\hat{\boldsymbol{x}}(t)}$は式(3.18)、(3.19)のような状態方程式で表せます。これを同一次元オブザーバと呼びます。

\dot{\hat{\boldsymbol{x}}}(t)=\boldsymbol{A} \hat{\boldsymbol{x}}(t)+B \boldsymbol{u}(t)-\boldsymbol{L}(\boldsymbol{\eta}(t)-\hat{\boldsymbol{\eta}}(t))
\tag{3.18}

\hat{\boldsymbol{\eta}}(t)=\boldsymbol{C} \hat{\boldsymbol{x}}(t)
\tag{3.19}

ここで${\boldsymbol{L} \in \mathbb{R}^{n \times r}}$はオブザーバゲインと呼ばれます。ここで、式(3.18)、(3.19)の同一次元オブザーバを用いたとき、推定誤差システムを考えます。

\dot{\varepsilon}=\dot{\boldsymbol{x}}(t)-\dot{\hat{\boldsymbol{x}}}(t)=(\boldsymbol{Ax}(t)+\boldsymbol{Bu}(t))-\{\boldsymbol{A} \hat{\boldsymbol{x}}(t)+B \boldsymbol{u}(t)-\boldsymbol{L}(\boldsymbol{\eta}(t)-\boldsymbol{C}\hat{\boldsymbol{x}}(t))\}

=(\boldsymbol{A}+\boldsymbol{LC})\varepsilon(t)
\tag{3.20}

式(3.20)より、推定誤差システムの固有値の実部が負になるよう、オブザーバゲイン${\boldsymbol{L}}$を設計することで、初期推定誤差${\varepsilon(0)=\varepsilon_0}$に対し、${t \rightarrow \infty}$で${\varepsilon(t) \rightarrow \boldsymbol{0}}$となります。つまり、${\boldsymbol{x}(t) \rightarrow \hat{\boldsymbol{x}}(t)}$となり、状態変数${\boldsymbol{x}(t)}$を推定することができます。

\varepsilon(t)=e^{(\boldsymbol{A}+\boldsymbol{LC})t} \varepsilon_0 \rightarrow \boldsymbol{0} \Rightarrow \boldsymbol{x}(t) \rightarrow \hat{\boldsymbol{x}}(t)
\tag{3.21}

この同一次元オブザーバを極配置法により設計します。極配置法とは、${\boldsymbol{A+LC}}$の固有値、つまり極を任意の指定した値に配置するよう、${\boldsymbol{L}}$を設計する手法です。今回はオブザーバの設計に利用しますが、コントローラ設計にも利用できますし、極を直接指定できるので便利です。極配置法についての詳細は参考文献を読むことをお勧めします(私の記事よりもよっぽど勉強になります)[1]-[3]。
${\boldsymbol{A+LC}}$の極${\boldsymbol{p}}$は次式のように設定しました。また、極の値の決め方について、${\boldsymbol{A+BK}}$の極の実部よりも${\boldsymbol{A+LC}}$の極の実部を負側に設定するようと良いです。これはコントローラが推定値${\hat{\boldsymbol{x}}(t)}$をフィードバックするため、オブザーバによる推定が遅いとコントローラに影響が出てしまうからです。

\boldsymbol{p}=
\begin{bmatrix}
-1500 & -300
\end{bmatrix}
\tag{3.22}

上記の極に配置するオブザーバゲイン${\boldsymbol{L}}$は次のような値になりました。

\boldsymbol{L}=
\begin{bmatrix}
-1.774\times10^3 & -4.046\times10^5
\end{bmatrix}^T
\tag{3.23}

下記に極配置法により、オブザーバゲインを求めるコードを示します。

place.py

# 極配置法
def place(A, C, p):
    place_result = signal.place_poles(A.T, C.T, p, method='YT')
    # オブザーバゲイン計算
    L = -place_result.gain_matrix.T
    # A+LCの極を計算
    eig_obsv_val, eig_obsv_vec = np.linalg.eig(A + L @ C)

    # 結果を表示
    print("p : ", p)
    print("L : \r\n", L)
    print("eig_obsv_val : ", eig_obsv_val)
    return L

出力結果

p :  [-1500  -300]
L :
 [[  -1774.4 ]
 [-404575.36]]
eig_obsv_val :  [-1500.  -300.]

3.3. カルマンフィルタ

カルマンフィルタだけでも一つ記事が書けてしまうので、今回は簡単な説明だけ行います。

3.3.1. カルマンフィルタとは

カルマンフィルタとは、逐次ベイズフィルタの一種であり、入力値と観測値からシステムの状態を推定するアルゴリズムです。それだけ聞くと、先述した同一次元オブザーバと同じですが、カルマンフィルタは入力値、観測値に白色雑音が混入していたとしても、尤もらしい状態を推定できます。今回は定常カルマンフィルタを利用した状態推定を行います。

まずカルマンフィルタのアルゴリズムについて説明します。初期値として、状態推定値の初期値${\hat{\boldsymbol{x}}(0)}$、事前誤差共分散行列の初期値${\boldsymbol{P}(0)}$、システム雑音の分散${\boldsymbol{Q}_v}$、観測雑音の分散${\boldsymbol{R}_w}$を与えたとき、予測ステップとフィルタリングステップを繰り返し、状態変数${\boldsymbol{x}(t)}$の推定値${\hat{\boldsymbol{x}}(t)}$を推定します。
式(2.8)、(2.9)より、制御対象の状態方程式を離散化した離散時間状態方程式は次式のように表されます。

\boldsymbol{x}[k+1]=\boldsymbol{A}_d \boldsymbol{x}[k]+\boldsymbol{B}_d \boldsymbol{u}[k]+\boldsymbol{B}_v \boldsymbol{v}[k]
\tag{3.24}

\boldsymbol{y}[k]=\boldsymbol{C}_d \boldsymbol{x}[k]+\boldsymbol{w}[k]
\tag{3.25}

このとき、${\boldsymbol{v}[k]}$は平均値0、分散${\boldsymbol{Q}_v}$の正規性白色雑音であり、システム雑音と呼ばれます。${\boldsymbol{w}[k]}$は平均値0、分散${\boldsymbol{R}_w}$の正規性白色雑音であり、観測雑音と呼ばれ、次のように表されます。ただし${\boldsymbol{v}[k]}$と${\boldsymbol{w}[k]}$は互いに独立とします。下記に予測ステップ、フィルタリングステップの時間更新式を示します。導出や詳しい説明は参考文献を読んでください。

\boldsymbol{E}(\boldsymbol{v}[k] \boldsymbol{v}^T[k])=\boldsymbol{Q}_v
\tag{3.26}

\boldsymbol{E}(\boldsymbol{w}[k] \boldsymbol{w}^T[k])=\boldsymbol{R}_w
\tag{3.27}

\boldsymbol{E}(\boldsymbol{v}[k] \boldsymbol{w}^T[k])=\boldsymbol{0}
\tag{3.28}

予測ステップ
事前状態推定値${\hat{\boldsymbol{x}}^-[k]}$、事前誤差共分散行列${\boldsymbol{P}^-[k]}$は次のように表されます。

\hat{\boldsymbol{x}}^-[k]=\boldsymbol{A}_d \hat{\boldsymbol{x}}[k-1]+\boldsymbol{B}_d \boldsymbol{u}[k-1]
\tag{3.29}

\boldsymbol{P}^-[k]=\boldsymbol{A}_d \boldsymbol{P}[k-1] \boldsymbol{A}_d^T+\boldsymbol{B}_v \boldsymbol{Q}_v \boldsymbol{B}_v^T
\tag{3.30}

フィルタリングステップ
カルマンゲイン${\boldsymbol{M}[k]}$、状態推定値${\hat{\boldsymbol{x}}[k]}$、事後誤差共分散行列${\boldsymbol{P}[k]}$は次のように表されます。

\boldsymbol{M}[k]=\boldsymbol{P}^-[k] \boldsymbol{C}_d^T(\boldsymbol{C}_d\boldsymbol{P}^-[k]\boldsymbol{C}_d^T+\boldsymbol{R}_w)^{-1}
\tag{3.31}

\hat{\boldsymbol{x}}[k]=\hat{\boldsymbol{x}}^-[k]+\boldsymbol{M}[k](\boldsymbol{y}[k]-\boldsymbol{C}_d \hat{\boldsymbol{x}}^-[k])
\tag{3.32}

\boldsymbol{P}[k]=(\boldsymbol{I}-\boldsymbol{M}[k]\boldsymbol{C}_d)\boldsymbol{P}^-[k]
\tag{3.33}

3.3.2. カルマンフィルタの状態空間表現

3.3.1節で説明したカルマンフィルタの形式では取り扱いしにくいので、カルマンフィルタの状態方程式を導出します。と言っても基本代入するだけです。式(3.29)に式(3.33)を代入します。

\hat{\boldsymbol{x}}^-[k+1]=\boldsymbol{A}_d \hat{\boldsymbol{x}}^-[k]+\boldsymbol{B}_d \boldsymbol{u}[k] + \boldsymbol{A}_d \boldsymbol{M}[k](\boldsymbol{y}[k]-\boldsymbol{C}_d \hat{\boldsymbol{x}}^-[k])
\tag{3.34}

式(3.34)の形を見ると式(3.18)の同一次元オブザーバと同じような形になります。式(3.34)は、離散オブザーバと呼ばれ、離散オブザーバゲイン${\boldsymbol{L}_d}$は次式となります。

\boldsymbol{L}_d=-\boldsymbol{A}_d \boldsymbol{M}[k]
\tag{3.35}

3.3.3. 定常カルマンフィルタ

先述したカルマンフィルタは、誤差共分散行列${\boldsymbol{P}}$を時変として考えていました。定常カルマンフィルタでは誤差共分散行列${\boldsymbol{P}}$を時不変として考えます。式(3.30)に(3.31)、(3.33)を代入すると次式のようになります。

\boldsymbol{P}^-[k]=\boldsymbol{A}_d [\boldsymbol{P}^-[k-1]-\boldsymbol{P}^-[k-1]\boldsymbol{C}_d^T(\boldsymbol{C}_d\boldsymbol{P}^-[k-1]\boldsymbol{C}_d^T+\boldsymbol{R}_w)^{-1}\boldsymbol{C}_d\boldsymbol{P}^-[k-1]]\boldsymbol{A}_d^T + \boldsymbol{B}_v \boldsymbol{Q}_v \boldsymbol{B}_v^T
\tag{3.36}

このとき、誤差共分散行列は時不変なので、${\boldsymbol{P}^-[k]=\boldsymbol{P}^-[k-1]=\boldsymbol{P}}$となります。ただし${\boldsymbol{P}^-[k]}$は正定値対称行列とします。したがって、式(3.36)は式(3.37)のように整理されます。

\boldsymbol{P}=\boldsymbol{A}_d \boldsymbol{P}\boldsymbol{A}_d^T
-\boldsymbol{A}_d\boldsymbol{P}\boldsymbol{C}_d^T(\boldsymbol{C}_d\boldsymbol{P}\boldsymbol{C}_d^T+\boldsymbol{R}_w)^{-1}(\boldsymbol{A}_d\boldsymbol{P}\boldsymbol{C}_d^T)^T + \boldsymbol{B}_v \boldsymbol{Q}_v \boldsymbol{B}_v^T
\tag{3.37}

式(3.37)は離散時間代数リカッチ方程式と呼ばれ、プログラムで簡単に解くことができます。式(3.37)の正定値解を${\boldsymbol{P}^*}$とすると、式(3.31)、(3.35)より、定常カルマンフィルタの離散オブザーバゲイン${\boldsymbol{L}_d}$は次のように表されます。

\boldsymbol{L}_d=-\boldsymbol{A}_d \boldsymbol{P}^* \boldsymbol{C}_d^T(\boldsymbol{C}_d\boldsymbol{P}^*\boldsymbol{C}_d^T+\boldsymbol{R}_w)^{-1}
\tag{3.38}

3.3.3. システム雑音・観測雑音

3.3.1節と3.3.2節では、カルマンフィルタのアルゴリズムやカルマンゲイン、オブザーバゲインの求め方について説明しました。これまで何度かカルマンフィルタを学ぶ機会があり、そのたびに「推定の手法は理解できたけれど、システム雑音の分散や観測雑音の分散がどう決まるのか、特にシステム雑音の分散がよくわからない」と悩むことが多くありました。そこで今回は、これらの分散を試行錯誤ではなく、体系的に決定してみようと思います。

図3.1に離散時間状態方程式のブロック線図を示します。システムの入力${\boldsymbol{u}[k]}$と同じように入力されている${\boldsymbol{v}[k]}$と、出力${\boldsymbol{y}[k]}$に加算されている${\boldsymbol{w}[k]}$があります。これらをそれぞれシステム雑音と観測雑音と呼びます。これらの説明は書きに示します。

システム雑音

システム雑音は、システム内部の不確かさや外部の影響によって発生する正規性白色雑音です。今回だとモデル化誤差や考慮していない静止摩擦力、モータドライバの入力電圧にのる電気的な雑音など、様々なものが考えられ、駆動源雑音とも呼ばれています。

観測雑音

観測雑音は、観測器やセンサの信号にのる電気的な雑音、温度変化や振動など周囲の環境から発生する環境雑音などによって発生する正規性白色雑音です。いわゆるセンサノイズ的なやつです。


図3.1. 離散時間状態方程式のブロック線図

このシステム雑音と観測雑音ですが、今まで分散を適当に決めてきた人間からすると、実際のシステムから分散を決定するのは大変です。システム雑音の分散はそもそもよくわからないし、観測雑音の分散も、エンコーダの分解能から計算できそうですが、量子化誤差は正規分布に基づいた雑音ではないです。色々考えた結果、システム雑音の分散は、前回のシステム同定で使ったデータを用いて、分散を計算しました。観測雑音の分散は、仕方がないのでエンコーダの量子化誤差から一様分布の分散を計算し、適用しました。この場合、カルマンフィルタの最適性や性能に影響を及ぼす可能性がありますが、カルマンフィルタを信じることにしました(~~実際適当に決めた分散でも動きますし~~)。

システム雑音の分散

状態変数を${\boldsymbol{x}(t)}$、推定値を${\hat{\boldsymbol{x}}(t)}$としたとき、誤差${\boldsymbol{e}(t)}$を式(3.36)、分散${\boldsymbol{Q}'_v}$をは式(3.37)に示します。

\boldsymbol{e}(t) = \boldsymbol{x}(t) - \hat{\boldsymbol{x}}(t)
\tag{3.39}

\boldsymbol{Q}'_v = \boldsymbol{\rm{E}}[\boldsymbol{e}(t) \boldsymbol{e}^T(t)]
\tag{3.40}

推定モデルが実システムの応答を表現できているとき、${\boldsymbol{e}(t)} \rightarrow \boldsymbol{0}$となります。この誤差${\boldsymbol{e}(t)}$の平均が${\boldsymbol{0}}$だと仮定すると、誤差が発生する原因はシステム雑音${\boldsymbol{v}[k]}$と考えられます。しかし、式(3.40)で求めた分散${\boldsymbol{Q}'_v}$は、${\boldsymbol{Q}_v=\boldsymbol{Q}'_v}$なのでしょうか？
上記の分散は状態変数${\boldsymbol{x}(t)}$と推定値${\hat{\boldsymbol{x}}(t)}$から求めました。つまり${\boldsymbol{B}_v}$の影響を受けた分散となります。今回は簡単のため、${\boldsymbol{B}_v=\boldsymbol{B}_d}$と考えると、分散${\boldsymbol{Q}'_v}$は次のように表されると考えられます。

\boldsymbol{Q}'_v=\boldsymbol{B}_d \boldsymbol{Q}_v \boldsymbol{B}^T_d
\tag{3.41}

以上より、本システムでのシステム雑音の分散${\boldsymbol{Q}'_v}$は次のように求まりました。

\boldsymbol{Q}'_v
=
\begin{bmatrix}
7.971 \times 10^{-2} & -9.111 \times 10^{-4}\\
-9.111 \times 10^{-4} & 3.388 \times 10^{0}
\end{bmatrix}
\tag{3.41}

観測雑音の分散

エンコーダの量子化誤差を${q_e}$、エンコーダの分解能を${\delta}$としたとき、量子化誤差${q_e}$は次のような範囲で一様に分布していると仮定します。

-\frac{\delta}{2} \leq q_e \leq \frac{\delta}{2}
\tag{3.42}

この範囲内での量子化誤差の確率密度関数${f(q_e)}$は、次のように表されます。

f(q_e) = 
\begin{cases}
  \frac{1}{\delta} & (-\frac{\delta}{2} \leq q_e \leq \frac{\delta}{2}) \\
  0 & (q_e < -\frac{\delta}{2},\quad q_e > \frac{\delta}{2})
\end{cases}
\tag{3.43}

証明は省略しますが、一様分布の分散は式(3.44)のように表され、これを観測雑音の分散${\boldsymbol{R}_w}$とします。

{\rm{Var}}[q_e]=\frac{\delta^2}{12} \approx \boldsymbol{R}_w
\tag{3.44}

分解能${\delta=2.618 \times 10^{-3} \mathrm{[rad/pulse]}}$とすると、観測雑音の分散${\boldsymbol{R}_w}$は式(3.45)となります。

\boldsymbol{R}_w = 5.712\times10^{-7}
\tag{3.45}

以上より、システム雑音の分散${\boldsymbol{Q}'_v}$、観測雑音の分散${\boldsymbol{R}_w}$をそれぞれ式(3.41)、(3.45)とすると、定常カルマンフィルタの離散オブザーバゲイン${\boldsymbol{L}_d}$は式(3.46)となります。

\boldsymbol{L}_d=
\begin{bmatrix}
-1.001\times10^0 & -7.751\times10^{-1}
\end{bmatrix}^T
\tag{3.46}

下記に離散時間代数リカッチ方程式を解き、定常カルマンフィルタのオブザーバゲインを求めるプログラムを示します。システムはゼロ次ホールドにて離散化しました。

SteadyState_KalmanFilter.py

# 定常カルマンフィルタ
def SteadyState_KalmanFilter(Ad, Cd, Q, R):
    P = solve_discrete_are(Ad.T, Cd.T, Q, R)
    Ld = -(Ad @ P @ Cd.T) @ np.linalg.inv(Cd @ P @ Cd.T + R)

    print("Qv : \r\n", Q)
    print("Rw : ", R)
    print("Ld : \r\n", Ld)
    return Ld

出力結果

Qv : 
 [[ 7.971e-02 -9.111e-04]
 [-9.111e-04  3.388e+00]]
Rw :  [[5.712e-07]]
Ld :
 [[-1.00077799]
 [-0.77514234]]

4. シミュレーション

3章で求めたコントローラゲイン、オブザーバゲインを用いて、本システムに適用できるかをシミュレーションにて確認します。

4.1. シミュレーション条件

シミュレーションは4次のRunge-Kutta法を用いて計算しました。設計時に使用した線形モデルおよび非線形モデルの2種類を適用し、コントローラとオブザーバの性能を評価しました。シミュレーション条件を表4.1に示します。

オブザーバは、極配置法を用いたものと定常カルマンフィルタの2種類を比較し、それぞれの特性を評価しました。ただし、極配置法を用いたオブザーバについては、マイコンでの実装を容易にするため、計算にはEuler法を採用しています。

さらに、エンコーダの観測値やマイコンの入力に生じる量子化誤差および雑音の影響を評価するため、以下の2つの条件を設定しました。

量子化や雑音が存在しない理想的な環境
量子化と雑音の両方が存在する環境

各条件下でシミュレーションを実施し、観測値および入力の量子化誤差やシステム雑音、観測雑音が制御系に与える影響を確認し、評価します。

表4.1. シミュレーション条件

項目	単位	値
シミュレーション周期 ${dt_{sim}}$	${\rm{s}}$	${1\times10^{-5}}$
シミュレーション時間 ${T_{sim}}$	${\rm{s}}$	${3.0}$
制御周期 ${dt}$	${\rm{s}}$	${1\times10^{-3}}$
初期角度 ${x_0}$	${\rm{rad}}$	${0}$
初期角速度 ${\dot{x}_0}$	${\rm{rad/s}}$	${0}$
目標角度 ${x_{ref}}$	${\rm{rad}}$	${\pi/2}$
最大入力電圧 ${V_{max}}$	${\mathrm{V}}$	${12}$
システム雑音の分散 ${\boldsymbol{Q}_v}$	${\rm{V}^2}$	${2.182\times10^{-3}}$
観測雑音の分散 ${\boldsymbol{R}_w}$	${\rm{rad}^2}$	${5.712\times10^{-7}}$
入力分解能 ${\delta_u}$	${\mathrm{V}}$	${5.859375 \times 10^{-3}}$
エンコーダ分解能 ${\delta}$	${\mathrm{rad/pulse}}$	${2.618 \times 10^{-3}}$

下記にコントローラやオブザーバなど、制御系のシミュレーションを行うコードを示します。このコードを用いてシミュレーションを行いました。

ControlSystemSimulator.py

# 制御システム
class ControlSystemSimulator:
    """
    制御システムのシミュレータ。
    """
    def __init__(self, system_function, dt, dt_sim, A=None, B=None, C=None, K=None, L=None, control_mode="state_feedback", is_discrete=False):
        """
        制御システムシミュレータの初期化

        system_function : システムの運動方程式 (関数: f(x, u))
        dt : 制御周期
        dt_sim : シミュレーション周期
        A : システムの状態行列 (オブザーバに使用)、離散オブザーバを使うときは離散化したシステム行列を渡して
        B : 入力行列
        C : 観測行列
        K : ゲイン行列 (状態フィードバックまたはサーボ系で使用)
        L : オブザーバゲイン
        is_discrete : 離散時間システムかどうか
        """
        self.system_function = system_function
        self.dt = dt
        self.dt_sim = dt_sim
        self.A = A
        self.B = B
        self.C = C
        self.K = K
        self.L = L
        self.is_discrete = is_discrete
        self.integral_error = 0.0  # 偏差の積分値
        self.control_mode = control_mode  # デフォルトは通常の状態フィードバック
        self.vmax = None  # 入力制限値（デフォルトは制限なし）

    def set_control_mode(self, mode):
        """
        制御モードの切り替え
        mode: "state_feedback" または "servo"
        """
        if mode not in ["state_feedback", "servo"]:
            raise ValueError("Control mode should be either 'state_feedback' or 'servo'.")
        self.control_mode = mode

    def set_input_limit(self, vmax):
        """
        入力制限の設定
        vmax: 入力の最大絶対値
        """
        self.vmax = vmax

    def reset_integral_error(self):
        """
        偏差の積分値をリセット
        """
        self.integral_error = 0.0

    def runge_kutta_4th_step(self, x, u):
        """
        ルンゲクッタ法による1ステップの計算
        x: 状態ベクトル
        u: 制御入力
        """
        k1 = self.system_function(x, u)
        k2 = self.system_function(x + 0.5 * self.dt_sim * k1, u)
        k3 = self.system_function(x + 0.5 * self.dt_sim * k2, u)
        k4 = self.system_function(x + self.dt_sim * k3, u)
        return x + (self.dt_sim / 6) * (k1 + 2 * k2 + 2 * k3 + k4)

    def state_feedback(self, x, x_ref=None):
        """
        状態フィードバック制御器
        x: 状態ベクトル
        x_ref: 目標値 (積分型最適サーボ系で使用)
        """
        if self.K is None:
            raise ValueError("Gain K is not set.")

        if self.control_mode == "state_feedback":
            # 通常の状態フィードバック
            u = self.K @ x
        elif self.control_mode == "servo":
            # 積分型最適サーボ系
            if x_ref is None:
                raise ValueError("The servo system requires a target value x_ref.")
            error = x_ref[0] - x[0]  # 偏差 (位置のみを考慮)
            self.integral_error += error * self.dt
            xe = np.block([[x[0], x[1], self.integral_error]]).T
            u = self.K @ xe

        # 入力制限を適用
        if self.vmax is not None:
            u = np.clip(u, -self.vmax, self.vmax)

        return u

    def state_observer(self, x_hat, y, u):
        """
        状態推定器の更新
        x_hat: 推定値
        y: 観測値
        u: 入力
        """
        if self.A is None or self.B is None or self.C is None or self.L is None:
            raise ValueError("System matrix or observer gain is not set.")

        ut = np.block([[u]])
        if self.is_discrete:
            # 離散時間オブザーバ
            x_hat = self.A @ x_hat + self.B @ ut - self.L @ (y - self.C @ x_hat)
        else:
            # 連続時間オブザーバ
            dx_hat = self.A @ x_hat + self.B @ ut - self.L @ (y - self.C @ x_hat)
            x_hat += dx_hat * self.dt
        return x_hat

4.2. シミュレーション結果

量子化や雑音が存在しない理想的な環境下の各評価指標を表4.2、線形モデルの応答を図4.1、線形モデルの応答を図4.2に示します。また量子化と雑音の両方が存在する環境下の各評価指標を表4.3、線形モデルの応答を図4.3、線形モデルの応答を図4.4に示します。青線が極配置法により設計したオブザーバで動作させたときのシステムの状態、オレンジ線が極配置法により設計したオブザーバの推定値、緑線がカルマンフィルタで動作させたときの状態、赤線がカルマンフィルタの推定値となります。

表4.2、図4.1、4.2より、量子化や雑音のない理想的な環境下においては、線形・非線形共に、極配置法により設計したオブザーバを利用した方が、すべての評価指標において性能が高いです。しかし非線形モデルの場合、極配置法により設計したオブザーバ、カルマンフィルタ共に、動摩擦トルクの影響から角速度が ${0\rm{[rad]}}$ にはなりません。このことからオブザーバによる推定値はモデル化誤差や考慮していない外乱による影響を強く受けることがわかります。
次に表4.3、図4.3、4.4を見ると、極配置法により設計したオブザーバは、量子化や雑音のある環境下において、角速度の推定値に雑音が発生していることがわかります。そのため、入力電圧もかなり振動的になっており、これを実機に入力すると考えると、現実的ではありません。
一方、カルマンフィルタの推定値は、極配置法により設計したオブザーバと比べて、雑音環境下においても正確に推定ができており、雑音に強いことがわかります。そのため、入力値も比較的安定しており、実機に入力しても問題なさそうです。

表4.2. 量子化や雑音が存在しない理想的な環境下

評価指標	単位	極配置(線形)	カルマンフィルタ(線形)	極配置(非線形)	カルマンフィルタ(非線形)
ピーク時間	${\rm{s}}$	0.3884	0.3903	0.3886	0.3951
オーバーシュート	%	1.094	1.154	1.091	1.074
立ち上がり時間	${\rm{s}}$	0.1794	0.1814	0.1794	0.1838
整定時間	${\rm{s}}$	0.2777	0.2789	0.2780	0.2834
RMSE	${\rm{rad}}$	0.2574	0.2570	0.2578	0.2594


角度、角速度の結果

入力電圧
図4.1. 量子化や雑音が存在しない理想的な環境下での結果(線形)


角度、角速度の結果

入力電圧
図4.2. 量子化や雑音が存在しない理想的な環境下での結果(非線形)

表4.3. 量子化と雑音の両方が存在する場合

評価指標	単位	極配置(線形)	カルマンフィルタ(線形)	極配置(非線形)	カルマンフィルタ(非線形)
ピーク時間	${\rm{s}}$	0.3713	0.3889	0.3861	0.3964
オーバーシュート	%	1.679	1.153	1.508	1.090
立ち上がり時間	${\rm{s}}$	0.1789	0.1815	0.1779	0.1838
整定時間	${\rm{s}}$	0.2698	0.2790	0.2739	0.2834
RMSE	${\rm{rad}}$	0.2592	0.2570	0.2594	0.2594


角度、角速度の結果

入力電圧
図4.3. 量子化と雑音の両方が存在する場合での結果(線形)


角度、角速度の結果

入力電圧
図4.4. 量子化と雑音の両方が存在する場合での結果(非線形)

5. 実機実験

この章では、3章で求めたコントローラゲイン、オブザーバゲインを用いて、本システムに適用できるかを実機実験にて確認します。

5.1. 実験条件

実験は、第1章で紹介した実験装置を使用し、シミュレーションと同様に、極配置法で設計したオブザーバと定常カルマンフィルタの2種類を比較します。それぞれの実験は同一条件で3回ずつ実施します。角度の測定にはエンコーダを使用し、角速度はエンコーダで得られた角度データから中心差分法を用いてオフラインで計算しました。
シミュレーションで実装していたコントローラやオブザーバについては、マイコン上で動作するようにC言語で記述しました。ただし、基本的なアルゴリズムはシミュレーションと同じため、特に難しいことはありません。
今回は次元数が少ない2次元のシステムであるため、行列を展開し、要素ごとに計算を行いました。この要素ごとの計算式については、事前にPythonを用いてシンボリック計算を行い、導出しています。以下にコードを示します。

state_feedback.c

// 状態フィードバック制御
int16_t state_feedback(double x, double dx, double x_ref) 
{
  static double integral_error = 0.0;
  double error = x_ref - x;
  integral_error = constrain(integral_error + error * dt, -VMAX, VMAX);

  double u = K0 * x + K1 * dx + G * integral_error;
  u = constrain(u, -VMAX, VMAX);

  int16_t duty = (int16_t)(u * DUTY_MAX / VMAX);
  duty = constrain(duty, -DUTY_LIM, DUTY_LIM);

  vout = u;
  return isfinite(vout) ? duty : 0;
}

state_observer.c

// 状態オブザーバ
void state_observer(double y, double u, bool is_discrete_flag) 
{
  if (is_discrete_flag) 
  {
    theta_hat[0] = (theta_hat[1] - y) * Lk0 + 1.953e-5 * u + theta_hat[1] + 0.000987 * dtheta_hat[1];
    dtheta_hat[0] = (theta_hat[1] - y) * Lk1 + 0.0389 * u + 0.9747 * dtheta_hat[1];
  } 
  else 
  {
    theta_hat[0] += ((theta_hat[1] - y) * Lp0 + dtheta_hat[1]) * dt;
    dtheta_hat[0] += ((theta_hat[1] - y) * Lp1 + 39.4 * u - 25.6 * dtheta_hat[1]) * dt;
  }
  theta_hat[1] = theta_hat[0];
  dtheta_hat[1] = dtheta_hat[0];
}

5.2. 実験結果

極配置法、定常カルマンフィルタ、各実験結果を表5.1、5.2、応答を図5.1、5.2に示します。実線は実際の応答を、点線は推定値を表しています。また、青、オレンジ、緑の各線色は、それぞれ1回目、2回目、3回目の実験結果を示しています。
極配置法は速い応答速度を持ち、立ち上がりが若干速いです。しかし角度は整定せず、オーバーシュートが大きいです。また入力がかなり振動的となっており、実験機にかかる負荷が大きいです。
定常カルマンフィルタは、オーバーシュートが小さく、整定時間が短いです。また滑らかな入力、応答になっているため、実験機にかかる負荷が小さく、実用的です。
以上より、実用的な観点から定常カルマンフィルタを用いた出力フィードバックが優れていると考えます。ただし、応答速度が重要で、許容角度が大きい場合、オブザーバゲインの調整次第では極配置法も適用可能だと考えます。

表5.1. 極配置法の結果

評価指標	単位	実験1	実験2	実験3	平均値
ピーク時間	${\rm{s}}$	0.4060	0.4330	0.3260	0.3883
オーバーシュート	%	6.335	5.335	5.666	5.779
立ち上り時間	${\rm{s}}$	0.1740	0.1710	0.1650	0.1700
整定時間	${\rm{s}}$	未整定	未整定	未整定	-
RMSE	${\rm{rad}}$	0.2670	0.2617	0.2665	0.2650


角度、角速度の結果

入力電圧
図5.1. 極配置法の応答

表5.2. 定常カルマンフィルタの結果

評価指標	単位	実験1	実験2	実験3	平均値
ピーク時間	${\rm{s}}$	0.4810	0.3490	0.3880	0.4060
オーバーシュート	%	1.834	1.000	1.331	1.388
立ち上り時間	${\rm{s}}$	0.1900	0.1900	0.1890	0.1897
整定時間	${\rm{s}}$	0.2920	0.3070	0.2950	0.2980
RMSE	${\rm{rad}}$	0.2551	0.2533	0.2518	0.2534


角度、角速度の結果

入力電圧
図5.2. 定常カルマンフィルタの応答

まとめ

今回は、前回同定したモデルを用いてアーム角度をサーボ制御してみました。また定常カルマンフィルタの設計をはじめてちゃんとできたと思うので、個人的にはおもしろかったです。
一方で、一般的な制御設計の手順をたどったつもりですが、記事が長くなってしまったので、次回書くなら短めの記事にしたいです。ただ今回の記事は、私の備忘録も兼ねていたので書けてよかったです。
最後まで見てくださりありがとうございました。ぜひ参考文献も見てみて下さい、とても参考になります。

最後に使用したコードのリンクを貼っておきますのでもしよかったら見てみて下さい。

参考文献

川田昌克，“MATLAB/Simulinkによる現代制御入門”，森北出版株式会社，(2019)
川田昌克(編著)，東俊一，市原裕之，浦久保孝光，大塚敏之，甲斐健也，國松禎明，澤田賢治，永原正章，南裕樹，“倒立振子で学ぶ制御工学”，森北出版株式会社，(2018)
南裕樹，“Pythonによる制御工学入門”，株式会社オーム社，(2024)
足立修一，丸田一郎，“カルマンフィルタの基礎”，東京電機大学出版局，(2022)
大住晃，亀山建太朗，松田吉隆，“カルマンフィルタとシステムの同定　ー動的逆問題へのアプローチー”，森北出版株式会社，(2021)
MathWorks，“カルマンフィルタ― -MATLAB & Simulink”，https://jp.mathworks.com/discovery/kalman-filter.html
(参照 2024年11月29日)
@rotace，“時変カルマンフィルタと定常状態カルマンフィルタの理論と実践”，Qiita，(2020)，https://qiita.com/rotace/items/351fe0a0a7a550545cd8 (参照 2024年11月29日)
鈴木宙見，杉江俊治，“観測雑音を含む量子化入出力データに基づくシステム同定”，システム制御情報学会論文誌，Vol20，No.5，p.177-184，(2007)
宮本克巳，“量子化誤差”，精密機械，44巻，526号，p.1249-1251，(1978)