1. 概要
手軽に実験できるシンプルでコンパクトな制御系をArduino Nanoで作ります。
2. ハードウェア
本環境はArduino Nanoに抵抗と電解コンデンサを2個ずつ接続した回路です。以下に外観図及び回路図を示します。
この回路はD3ピン(PWM)を入力、A5ピンを出力とするほか、A6ピンを中間出力として備えています。回路定数から導かれる状態空間表現の一つは
\frac{\mathrm{d}}{\mathrm{d}t}
\left[
\begin{array}{c}
V_{o}(t)\\ V_{1}(t)
\end{array}
\right]=\left[
\begin{array}{cc}
-\frac{1}{C_2R_2} & \frac{1}{C_2R_2}\\
\frac{1}{C_1R_2} & -\frac{1}{C_1}\left(\frac{1}{R_1}+\frac{1}{R_2}\right)
\end{array}
\right]\left[
\begin{array}{c}
V_{o}(t)\\ V_{1}(t)
\end{array}
\right]+\left[
\begin{array}{c}
0 \\ \frac{1}{C_1R_1}
\end{array}
\right]V_i(t)\tag{1}
であり、これにより2つの出力を状態変数として取ることで全状態フィードバックを可能としています。Arduino Nanoの電源は単極性0~5Vで、AD変換は10ビット(0~5Vを0~1023に変換)、PWMは8ビット(0~255を0~5Vに変換)の分解能を持ちます。
3. ソフトウェア
制御器の計算を行うため、Timer1による定周期割り込みを行うテンプレートコードは以下の通りです。本コードではcontrol関数がTimer1.initializeで指定した間隔でコールされるため、この関数内に制御則を実装します。またデータ出力のためserOut関数にてPCへ状態と入力を定周期出力します。
また、前述の通りArduinoの電源は単極性のため、負電圧を入出力できません。このため原点を$V_o=V_1=0$とすると、特にレギュレータ問題などで原点付近に非線形なクリッピングが発生する可能性があります。対策として、原点の平行移動を行います。漸近安定な連続時間線形システム
\frac{\mathrm{d}}{\mathrm{d}t}x(t)=Ax(t)+Bu(t)
に対し定値入力$\bar{u}$を与えたときの定常状態を$\bar{x}$とします。線形性により
\frac{\mathrm{d}}{\mathrm{d}t}(x(t)+\bar{x})=A(x(t)+\bar{x})+B(u(t)+\bar{u})
であるため、$\bar{x}$を原点としたときの変動は元の状態方程式に従います。(1)より入力電圧を一定値$V_i(t)\equiv \bar{V_i}$としたとき、状態変数はともに$V_o=V_1=\bar{V_i}$に収束します。このためPWM出力を0~5Vの中間値2.5V(128)だけ加算し、AD変換の入力値から2.5V(511)を引くことで等価的に正負入出力を扱うことができます。
#include <TimerOne.h>
// ピン設定
#define INPUT_PIN 3
#define OUTPUT_PIN_1 6
#define OUTPUT_PIN_2 5
// 状態変数
float V1, Vo;
// 入力
float Vi;
// シリアル出力
void serOut(){
Serial.print(Vi, 4);
Serial.print(',');
Serial.print(V1, 4);
Serial.print(',');
Serial.println(Vo, 4);
}
// AD値(0~1023)を-2.5~2.5Vに変換
float convDac(float y){
return (y - 511) / 1023.0 * 5.0;
}
// -2.5~2.5VをPWM値(0~255)に変換
int convPwm(float u){
int ret = 255/5*u + 128;
if (ret > 255)
ret = 255;
else if (ret < 0)
ret = 0;
return ret;
}
// 制御則の実装
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
// 制御演算
// Vi = ...
analogWrite(INPUT_PIN, convPwm(Vi));
serOut();
}
void setup() {
Serial.begin(/* Baudレート */);
Timer1.initialize(/* サンプリング周期 */);
Timer1.attachInterrupt(control);
Timer1.start();
}
void loop() {
}
4. 使用例
以下、本環境を使用した実験例を示します。
4.1 システム同定
M系列信号を入力としたシステム同定実験を行います。M系列は2値(0,1)の疑似白色雑音であり、シフトレジスタの特定ビットのXOR演算により生成することができます。この例では7ビットのシフトレジスタの第1,第7ビットのXORに基づいて入力電圧$V_i$を決定する処理を関数mseq内に実装しています。
実験データはシリアル通信で出力されるため、Arduino IDEのシリアルモニタをコピペするかTera Term等で記録することができます。
// M系列更新周期
#define MSEQ_WIDTH 40
// M系列の変換
char mseq(){
// シフトレジスタ
static unsigned char Xn = 1;
static unsigned int cnt = MSEQ_WIDTH;
unsigned ret;
cnt--;
ret = ((Xn & 64) >> 6) ^ (Xn & 1);
if (cnt == 0){
Xn = (Xn << 1) + ret;
cnt = MSEQ_WIDTH;
}
return ret;
}
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
// M系列(0,1)を-2V,+2Vに変換
Vi = 4.0 * (mseq() - 0.5);
analogWrite(INPUT_PIN, convPwm(Vi));
serOut();
}
取得したデータは適当な同定アルゴリズムにより処理することができます。今回は以下のJuliaコードで回路定数を用いたホワイトボックスモデリングと最小二乗法によるシステム同定を実施しました。最小二乗法の正規方程式は状態方程式(1)を離散化した2次系のパラメータを
\left[
\begin{array}{c}
V_{o}[k+1]\\ V_{1}[k+1]
\end{array}
\right]=\left[
\begin{array}{cc}
A_{11} & A_{12}\\
A_{21} & A_{22}
\end{array}
\right]\left[
\begin{array}{c}
V_{o}[k]\\ V_{1}[k]
\end{array}
\right]+\left[
\begin{array}{c}
B_{1} \\ B_{2}
\end{array}
\right]V_i[k]\tag{2}
と仮定することで下式のように導かれます。
\left[
\begin{array}{c}
V_o[2]\\
V_1[2]\\
\vdots\\
V_o[N]\\
V_1[N]
\end{array}
\right]=\left[
\begin{array}{cccccc}
V_o[1]&V_1[1]&0&0&u[1]&0\\
0&0&V_o[1]&V_1[1]&0&u[1]\\
\vdots\\
V_o[N-1]&V_1[N-1]&0&0&u[N-1]&0\\
0&0&V_o[N-1]&V_1[N-1]&0&u[N-1]\\
\end{array}
\right]\left[
\begin{array}{c}
A_{11} \\ A_{12} \\ A_{21} \\ A_{22} \\ B_1 \\ B_{2}
\end{array}
\right]
using PyPlot
using CSV
# RK4 #
function rk4(x, u, dt, f)
k1 = f(x, u);
k2 = f(x + k1*dt/2, u);
k3 = f(x + k2*dt/2, u);
k4 = f(x + k3*dt, u);
return x + dt/6 * (k1 + 2*k2 + 2*k3 + k4);
end
# 最小二乗法によるシステム同定 #
function ident(Vi, Vo, V1)
N = length(Vi);
g = zeros(2*(N-1), 1);
F = zeros(2*(N-1), 6);
for n = 1:N-1
g[2*n-1:2*n] = [
Vo[n+1];
V1[n+1]
];
F[2*n-1:2*n,:] = [
Vo[n] V1[n] zeros(1, 2) Vi[n] 0;
zeros(1, 2) Vo[n] V1[n] 0 Vi[n]
];
end
theta = (F'*F)\(F'*g);
A = [
theta[1] theta[2];
theta[3] theta[4]
];
B = [theta[5]; theta[6]];
return A, B
end
function main()
# 同定実験データ読み込み #
dat = CSV.read("data.csv");
Vi = dat[1];
V1 = dat[2];
Vo = dat[3];
N = length(Vi);
n = 1:N;
dt = 50e-3;
# 回路定数から計算したモデル #
R1 = 2e3; C1 = 47e-6;
R2 = 20e3; C2 = 120e-6;
Am = [
-1/C2/R2 1/C2/R2;
1/C1/R2 -1/C1*(1/R1+1/R2)
];
Bm = [
0; 1/C1/R1
]
fm = (x,u) -> Am*x + Bm*u;
xm = zeros(2, N);
# 最小二乗法によるフィッティング $
(Ai, Bi) = ident(Vi, Vo, V1);
display("Ai = ");
display(Ai);
display("Bi = ");
display(Bi);
xi = zeros(2, N);
for n = 1:N-1
xm[:,n+1] = rk4(xm[:,n], Vi[n], dt, fm);
xi[:,n+1] = Ai*xi[:,n] + Bi*Vi[n];
end
# プロット #
close("all");
figure();
plot(n, Vi, label = L"V_i");
plot(n, V1, label = L"V_1");
plot(n, Vo, label = L"V_o");
title("Identification Data");
xlabel("step");
ylabel("voltage [V]");
legend();
figure();
subplot(2,1,1);
plot(n, Vo, label = "measured");
plot(n, xm[1,:], label = "whitebox");
plot(n, xi[1,:], label = "least square");
ylabel(L"V_o");
legend();
title("Identification Result");
subplot(2,1,2);
plot(n, V1);
plot(n, xm[2,:]);
plot(n, xi[2,:]);
xlabel("step");
ylabel(L"V_1");
end
各種法による同定結果は下図のようになり、おおむね一致した結果を得ました。せっかくなので次節以降の設計には必要に応じて最小二乗法による結果
\left[
\begin{array}{c}
V_{o}[k+1]\\ V_{1}[k+1]
\end{array}
\right]=\left[
\begin{array}{cc}
0.98168 & 0.0131552\\
0.0403022& 0.552701
\end{array}
\right]\left[
\begin{array}{c}
V_{o}[k]\\ V_{1}[k]
\end{array}
\right]+\left[
\begin{array}{c}
0.00494626 \\ 0.4060038
\end{array}
\right]V_i[k]\tag{2}
を用いることにします。
4.3 P制御、PI制御
P制御$K_p=1.0$の実装例は以下の通りです。M系列の計算処理を流用し±2Vのステップ状目標値$V_r$を生成し、$V_o$を追従させることを目標とします。
// M系列の更新周期
#define MSEQ_WIDTH 160
// 目標値
float Vr;
// Kp = 1のP制御
float p_ctrl(float e){
return 1.0 * e;
}
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vr = 4.0 * (mseq() - 0.5);
Vi = p_ctrl(Vr - Vo);
analogWrite(INPUT_PIN, convPwm(Vi));
serOut();
}
結果はよく知られた通り定常偏差が残留しています。
定常偏差の改善のため、PI制御$(K_p=1.0,K_i=0.3)$を実装します。
// Kp = 1.0, Ki = 0.3
float pi_ctrl(float e){
static float esum = 0.0;
esum += e * 50e-3;
return 1.0 * e + 0.3*esum;
}
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vr = 4.0 * (mseq() - 0.5);
Vi = pi_ctrl(Vr - Vo);
analogWrite(INPUT_PIN, convPwm(Vi));
serOut();
}
I制御の導入により十分時間が経過した後には定常偏差が解消されることが確認できました。
4.4 状態フィードバック
極配置による状態フィードバックを実装してみます。(2)の離散時間線形系について、極を$0.75\pm j0.1$に配置する状態フィードバック制御則は
V_i[k]=-\left[8.6039,-0.0201\right]\left[
\begin{array}{c}
V_o \\ V_1
\end{array}
\right]
で与えられます。レギュレータの効果を確かめるため、入力端外乱$V_n\in[-2.0,2.0]$を印加して実験を行いました。
float pole_place(float Vo, float Vi){
return - 8.6039*Vo + 0.0201*Vi;
}
void control() {
float Vn;
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vi = pole_place();
Vn = random(-20, 21) / 10.0;
analogWrite(INPUT_PIN, convPwm(Vi + Vn));
serOut();
}
以下に制御なしの場合と状態フィードバックを行った場合の時系列を示します。状態フィードバックにより外乱への感度が低減されています。
また状態オブザーバを用いた出力フィードバック極配置も試してみます。$V_o$を出力としたとき、オブザーバ極を$0.1,0.2$に配置するオブザーバゲインは$L=[1.0344,6.8154]^\mathsf{T}$で与えられます。
// 状態変数の推定値
float V1e, Voe;
float pole_place_obs(){
float V1e_new, Voe_new;
float Vi_now = - 8.6039*Voe + 0.0201*V1e;
Voe_new = 0.98168*Voe + 0.0131552*V1e + 0.00494626 * Vi + 1.0344 * (Vo - Voe);
V1e_new = 00.0403022*Voe + 0.552701*V1e + 0.4060038 * Vi + 6.8154 * (Vo - Voe);
Voe = Voe_new;
V1e = V1e_new;
return Vi_now;
}
void control() {
float Vn;
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vi = pole_place_obs();
Vn = random(-20, 21) / 10.0;
analogWrite(INPUT_PIN, convPwm(Vi + Vn));
serOut();
}
以下に状態変数の応答と、状態オブザーバの推定値と状態変数の観測値との比較結果を示します。$V_1$の推定に外乱の影響が顕著ですが、概ね良好に制御できているようです。
4.5 量子化制御
ArduinoのPWMピンはデジタル出力が可能です。せっかくなので$\left\{ -2.5,2.5 \right\}$に入力を制限した場合の挙動を実験してみます。制御器は4.3のPI制御とします。制御入力をデジタル出力とするため、setup関数内でpinModeを指定します。
void setup() {
pinMode(INPUT_PIN, OUTPUT);
Serial.begin(230400);
Timer1.initialize(50000);
Timer1.attachInterrupt(control);
Timer1.start();
}
連続値入力を離散入力に変換する機構を量子化器と呼びます。最も簡単には連続値を最近傍の離散値へ丸めればよく、このような量子化器を単純量子化器と呼びます。この場合、単純量子化器$Q_s:[-2.5,2.5]\to\left\{-2.5,2.5\right\}$は
V_{iq}=Q_s(V_i)=\left\{
\begin{array}{cc}
2.5&(V_i>0)\\
-2.5&(V_i\leq0)
\end{array}
\right.
と表されます。単純量子化器による量子化制御は以下のように実装されます。
// 量子化された入力
int Viq;
// {-2.5,2.5}を{0,1}に変換
int convDigitalWrite(float u){
if (u > 0)
return 1;
else
return 0;
}
// 単純量子化器
float Qs(float u){
if (u > 0)
return 2.5;
else
return -2.5;
}
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vr = 4.0 * (mseq() - 0.5);
Vi = pi_ctrl(Vr - Vo);
Viq = Qs(Vi);
digitalWrite(INPUT_PIN, convDigitalWrite(Viq));
serOut();
}
結果は以下の図のようになりました。入力が$\pm 2.5$Vに制約されたことで、オーバーシュートが発生し、大幅な応答の劣化が発生しています。(ワインドアップっぽい？)
次に、最適動的量子化器(東、杉江2007)による改善を試みます。動的量子化器は状態を持つ動的な量子化器で、特にプラントの状態空間表現が$(A,B,C)$で表されるとき、以下の動的量子化器は最大値ノルムの意味での最適性を持つことが示されています。
\begin{aligned}
\xi[k+1]&=A\xi[k]+B(V_{iq}[k]-V_i[k])\\
V_{iq}[k]&=Q_s\left(-\left(CB\right)^{-1}CA\xi[k]+V_i[k]\right)
\end{aligned}
float Qd(float u){
static float xi[] = {0, 0};
float xi0_new;
float v;
v = Qs(-(198.4691*xi[0]+2.6596*xi[1]) + u);
xi0_new = 0.98168*xi[0] + 0.0131552*xi[1] + 0.00494626 * (v-u);
xi[1] = 0.0403022*xi[0] + 0.552701*xi[1] + 0.4060038 * (v-u);
xi[0] = xi0_new;
return v;
}
void control() {
V1 = convDac(analogRead(OUTPUT_PIN_1));
Vo = convDac(analogRead(OUTPUT_PIN_2));
Vr = 4.0 * (mseq() - 0.5);
Vi = pi_ctrl(Vr - Vo);
Viq = Qd(Vi);
digitalWrite(INPUT_PIN, convDigitalWrite(Viq));
serOut();
}
実験結果を以下に示します。先程の例と同様に入力は${-2.5,2.5}$に制限されているにもかかわらず、良好な応答が得られました。
