1. コレは何？/What is this?

C++上で連続系状態方程式のA行列とB行列を離散化する。 ヘッダをインクルードするだけで使用可能！
MATLABでいうところのc2d関数に当たる。コンパイル時もしくはオンラインで状態方程式を離散化できるようになります！

2. Discretize離散化関数の機能

連続系状態方程式のA，B行列とサンプリング時間から，離散系状態方程式のA，B行列を計算する。

3. ダウンロード

ソースコード一式は以下からダウンロードできます。

Discretize-REV.10151105.zip

gcc 4.9.2 以上を推奨。とりあえずMSYS2+MinGW64で動作確認済み。
注意！C++11じゃないと動きません！！(一部，templateとconstexprを使用しているので)

4. ファイル一覧

Makefile ←メイクファイル
Makefile.depend ←依存関係ファイル
Matrix.cc ←Matrixクラスの実体
Matrix.hh ←Matrixクラスのヘッダ
DiscretizeTest.cc ←使い方の一例(2慣性共振系の離散化の例)
Discretize.hh ←離散化関数のヘッダ
Discretize.cc ←離散化関数の実体

Matrixクラスは必須です。Matrixクラスの説明は→こちら

5. コンパイルの前に…

コンパイルする前に，自分の処理系に合わせてMakefileの中のCFLAGSをいじって下さい。デフォルトではMSYS2+MINGW64用になってます。

6. コンパイル＆実行

make でコンパイル。その後に ./DiscretizeTest を起動すると，2慣性共振系の状態方程式の離散化例が表示されます。

7. 関数一覧

計算結果はMATLABと比較して合っていることを確認済み！

void Discretize(const Matrix& Ac, const Matrix& Bc, Matrix& Ad, Matrix& Bd, const double Ts);
// 連続系状態方程式のA，B行列を離散化する関数
// Ac：連続系のA行列，Bc：連続系のB行列，Ad：離散系のA行列，Bd：離散系のB行列，Ts：サンプリング時間

void Discretize(const Matrix& Ac, const Matrix& Bc, Matrix& Ad, Matrix& Bd,
                     const double Ts, const unsigned int Npade, const unsigned long Nint);
// 連続系状態方程式のA，B行列を離散化する関数
// Ac：連続系のA行列，Bc：連続系のB行列，Ad：離散系のA行列，Bd：離散系のB行列，
// Ts：サンプリング時間，Npade：パデ近似の次数，Nint：定積分の分割数

1つ目のDiscretize関数は，パデ近似の次数を6，定積分の分割数を1000として離散化する。
2つ目のDiscretize関数(オーバーロード)は，パデ近似の次数と定積分の分割数を任意に指定できる。好きな方を使ってネ。

8. 使用例

＜2慣性共振系の離散化＞

試しに，2慣性共振系の連続系状態方程式を離散化してみる。

C++のコード例

MATLABのコード例

// パラメータ
const double Ts = 100e-6; // [s]      サンプリング時間
const double Jm = 1e-4;   // [kgm^2]  モータ慣性
const double Jl = 1;      // [kgm^2]  負荷側慣性
const double Dm = 0.1;    // [Nms/rad]モータ粘性
const double Dl = 0.3;    // [Nms/rad]負荷側粘性
const double Ks = 500;    // [Nm/rad] 2慣性間のばね定数
const double Rg = 50;     //          減速比
const double Kt = 0.04;   // [Nm/A]   トルク定数

// 2慣性共振系の連続系状態方程式のA行列
Matrix Ac(3,3);
Ac.Set(
    -Dm/Jm,      0, -Ks/(Rg*Jm),
         0, -Dl/Jl,       Ks/Jl,
    1.0/Rg,     -1,           0
);
PrintMatrix(Ac,"%1.15e");

// 2慣性共振系の連続系状態方程式のB行列
Matrix Bc(2,3);
Bc.Set(
    Kt/Jm,       0,
        0, -1.0/Jl,
        0,       0
);
PrintMatrix(Bc,"%1.15e");

// 離散化
Matrix Ad(3,3), Bd(2,3);
Discretize(Ac,Bc,Ad,Bd,Ts);

// 結果表示
PrintMatrix(Ad,"%1.15e");   // 離散系状態方程式のA行列
PrintMatrix(Bd,"%1.15e");   // 離散系状態方程式のB行列

% パラメータ
Ts = 100e-6;% [s]       サンプリング時間
Jm = 1e-4;  % [kgm^2]   モータ慣性
Jl = 1;     % [kgm^2]   負荷側慣性
Dm = 0.1;   % [Nms/rad] モータ粘性
Dl = 0.3;   % [Nms/rad] 負荷側粘性
Ks = 500;   % [Nm/rad]  2慣性間のばね定数
Rg = 50;    %           減速比
Kt = 0.04;  % [Nm/A]    トルク定数

% 2慣性共振系の連続系状態方程式のA行列
Ac = [ -Dm/Jm,      0, -Ks/(Rg*Jm);
           0, -Dl/Jl,       Ks/Jl ;
      1.0/Rg,     -1,           0 ]

% 2慣性共振系の連続系状態方程式のB行列
Bc = [  Kt/Jm,       0;
            0, -1.0/Jl;
            0,       0]

% 出力方程式はどうでもいい。。。
Cc = [ 1, 0, 0 ];
Dc = [ 0, 0 ];

% 離散化
Sc = ss(Ac,Bc,Cc,Dc);
Sd = c2d(Sc,Ts);

% 結果表示
Ad = Sd.a   // 離散系状態方程式のA行列
Bd = Sd.b   // 離散系状態方程式のB行列

C++の実行結果は，

Ad =
[ 9.048280603753414e-01 4.837359211290931e-04 -9.516218351841864e+00 ]
[ 4.837359211290931e-08 9.999675005051197e-01 4.999904576097310e-02 ]
[ 1.903243670368373e-06 -9.999809152194623e-05 9.999878252141505e-01 ]

Bd =
[ 3.806490592347488e-02 -1.625805364281345e-08 ]
[ 6.503221457125377e-10 -9.999841668301928e-05 ]
[ 3.869926388361497e-08 4.999939747719074e-09 ]

そして，MATLABの実行結果は。。。。

Ad = 9.048280603753419e-01     4.837359211290942e-04    -9.516218351841895e+00
     4.837359211290944e-08     9.999675005051213e-01     4.999904576097325e-02
     1.903243670368379e-06    -9.999809152194650e-05     9.999878252141552e-01

Bd = 3.806490592347486e-02    -1.625805364281345e-08
     6.503221457125382e-10    -9.999841668301933e-05
     3.869926388361497e-08     4.999939747719076e-09

MATLABのc2d関数と13～14桁まで一致！ほぼ，イキかけました。コンパイル時もしくはオンラインで離散化ができる！！！モータ制御なんて有効数字2桁もあれば良い方なんで十分すぎる。
これで，c2d関数ともおさらばですな。

9. 中で何をやっているのか？

Discretize関数の中で何やってるかの説明を書いときます。まー，ほとんど教科書そのままなんですが，MATLABと同様にパデ近似を使用しているところがちょっと違います。

＜教科書に載ってる式＞

まず、連続系の状態方程式を、 \begin{eqnarray} \dot{\boldsymbol x}(t) &=& A_c {\boldsymbol x}(t) + B_c {\boldsymbol u}(t) \\ {\boldsymbol y}(t) &=& C_c {\boldsymbol x}(t) + D_c {\boldsymbol u}(t) \end{eqnarray} とします。ここでは、下付き添字の \( c \) は連続系を、\( d \) は離散系を示すこととします。教科書を開くと、以下の式が載っています。 \begin{eqnarray} A_d &=& e^{A_c T_s} \\ B_d &=& \int^{T_s}_0 e^{A_c t} dt B_c \end{eqnarray} ここで \( T_s \) はサンプリング時間(制御周期)です。 C行列とD行列は \( C_d = C_c, D_d = D_c \) としておｋ。これで離散化ができるわけだ。 (3)、(4)式を見ると、一見チョー簡単そうにみえます。でも、式中の \( e \) は行列指数関数(指数行列)なんですねえ、ただの指数関数じゃないんで、実は面倒。状態遷移行列ってやつです。

＜行列指数関数の計算方法I (マクローリン展開版)＞

じゃあ、\( e \) は計算機上でどうするのか、という話ですね。お手軽にマクローリン展開で済まそうと思えば、 \begin{equation} e^X = I + X + \frac{1}{2!}X^2 + \frac{1}{3!}X^3 + \frac{1}{4!}X^4 + \cdots \end{equation} となるわけですね。 \( X \) は入力引数の行列、\( I \) は単位行列。 (3)式，(4)式の場合はそれぞれ \( X = A_c T_s \)，\( X = A_c t \)と読み替えてネ。ただ、この方法は収束が遅くて実用的でない。

＜行列指数関数の計算方法II (パデ近似版)＞

なので、 \begin{equation} e^X = ( I - c_1 X + c_2 X^2 - c_3 X^3 + \cdots )^{-1} (I + c_1 X + c_2 X^2 + c_3 X^3 + \cdots ) \end{equation} のように計算します。これがパデ近似ってやつで、真値までの収束がめちゃ速い。 \( c \) はパデ近似の係数で、 \begin{equation} c_i = c_{i-1} \frac{m-i+1}{i (2m-i+1)} ~~~ (c_0 = 1, i = 1,2,3,\cdots) \end{equation} のように計算すればおｋ。ここで \( m \) はパデ近似の次数。指数関数の5次のパデ近似の場合は、 \begin{equation} [ c_1, c_2, c_3, c_4, c_5 ] = [ 1/2, 1/9, 1/72, 1/1008, 1/30240 ] \end{equation} となるはずなので、デバッグ時は見比べると良い。これで行列指数関数が計算できる！次数は5次とか6次もあれば，十分すぎるほどのようだ。ただし，(6)式から分かるように行列の乗算・加算・冪・逆行列の計算が必要となるので，それは行列/ベクトル計算クラスに任せます。

＜高精度な計算のために＞

実際には、入力行列 \( X \) の無限大ノルムを考慮してスケーリングをかけ、上記の計算をして、最後にスケーリングを元に戻すと精度よく計算できるようです。その辺の実装は Matrix.cc の expm関数の中身を見た方が早いかも。。。

＜シンプソン法による定積分＞

次に、(4)式の定積分について。定積分はシンプソンの公式が簡単でイイ。この部分は暇ができたら後々説明する予定。

＜離散系の状態方程式＞

最後に離散系の状態方程式が得られる。 \begin{eqnarray} {\boldsymbol x}(k+1) &=& A_d {\boldsymbol x}(k) + B_d {\boldsymbol u}(k) \\ {\boldsymbol y}(k) &=& C_d {\boldsymbol x}(k) + D_d {\boldsymbol u}(k) \end{eqnarray} これだけ。

10. 2慣性共振系のシミュレーション

せっかく作ったので，実際に2慣性共振系の挙動をDiscretize離散化関数を使って計算してみる。ソースコードは以下の通り。

＜クラス側の実装(重要な部分のみ抜粋)＞

こんな感じで，コンストラクタで連続系を定義＆離散化してしまう。状態方程式の計算もすげー簡単になる。ちょー便利。


     1 :	TwoInertiaSimulator::TwoInertiaSimulator(
     2 :	    double TrqConst, double MotorInert, double LoadInert, double Spring,
     3 :	    double MotorFric, double LoadFric, double GearRatio, double SmplTime
     4 :	    ) // コンストラクタ
     5 :	    : Kt(TrqConst), Jm(MotorInert), Jl(LoadInert), Ks(Spring),
     6 :	      Dm(MotorFric), Dl(LoadFric), Rg(GearRatio), Ts(SmplTime),
     7 :	      Ac(Matrix(3,3)), Bc(Matrix(2,3)),
     8 :	      Ad(Matrix(3,3)), Bd(Matrix(2,3)), C(Matrix(3,2)),
     9 :	      u(Matrix(1,2)), x(Matrix(1,3)), x_next(Matrix(1,3)), y(Matrix(1,2))
    10 :	{
    11 :	    // 連続系A行列の設定
    12 :	    Ac.Set(
    13 :	        -Dm/Jm,      0, -Ks/(Rg*Jm),
    14 :	             0, -Dl/Jl,       Ks/Jl,
    15 :	        1.0/Rg,     -1,           0
    16 :	    );
    17 :	    
    18 :	    // 連続系B行列の設定
    19 :	    Bc.Set(
    20 :	        Kt/Jm,       0,
    21 :	            0, -1.0/Jl,
    22 :	            0,       0
    23 :	    );
    24 :	    
    25 :	    // C行列の設定
    26 :	    C.Set(
    27 :	        1,  0,  0,
    28 :	        0,  1,  0
    29 :	    );
    30 :	    
    31 :	    // 離散化
    32 :	    Discretize(Ac,Bc,Ad,Bd,Ts);
    33 :	}
    34 :	
    35 :	void TwoInertiaSimulator::GetVelocity(double current, double loadtorque, double& motorspeed, double& loadspeed){
    36 :	    // モータ側＆負荷側速度を計算する関数
    37 :	    
    38 :	    // 入力ベクトルの設定
    39 :	    u.Set(
    40 :	        current,
    41 :	        loadtorque
    42 :	    );
    43 :	    
    44 :	    // 状態方程式の計算
    45 :	    x_next = Ad*x + Bd*u;
    46 :	    y = C*x;
    47 :	    
    48 :	    // 状態ベクトルの更新
    49 :	    x = x_next;
    50 :	    
    51 :	    // 出力ベクトルから抽出
    52 :	    motorspeed = y.GetElem(1,1);
    53 :	    loadspeed = y.GetElem(2,1);
    54 :	}

＜呼び出し側の実装(重要な部分のみ抜粋)＞

ARCSでの実装例。


     1 :	static TwoInertiaSimulator* PlantSim1;
     2 :	
     3 :	if(pCF->CmdFlag==CTRL_INIT){
     4 :	    // 初期化モード (ここは制御開始時に1度だけ呼び出される(非実時間空間なので重い処理もOK))
     5 :	    PlantSim1 = new TwoInertiaSimulator(Kt,Jm,Jl,Ks,Dm,Dl,Rg,Ts);
     6 :	}
     7 :	if(pCF->CmdFlag==CTRL_LOOP){
     8 :	    // 周期モード (ここは制御周期 SAMPLING_TIME[0] 毎に呼び出される(実時間空間なので処理は制御周期内に収めること))
     9 :	    pCF->count++;       // ループカウンタを進める
    10 :	    t=pCF->count*Ts;    // 時刻の計算
    11 :	    
    12 :	    // 制御ここから
    13 :	    
    14 :	    // ステップ信号の生成
    15 :	    if(t < 1) iqref = 0;
    16 :	    if(1 <= t && t < 2) iqref = 1;
    17 :	    if(2 <= t) iqref = 0;
    18 :	    
    19 :	    // ここで2慣性共振系を計算
    20 :	    PlantSim1->GetVelocity(iqref, 0, omega_m, omega_l);
    21 :	    
    22 :	    // 制御ここまで
    23 :	    
    24 :	    // データの保存用
    25 :	    pCF->Data[0] = t;   // [s]  時刻の保存
    26 :	    pCF->Data[1] = iqref;   // 保存変数
    27 :	    pCF->Data[2] = omega_m; // 保存変数
    28 :	    pCF->Data[3] = omega_l; // 保存変数
    29 :	    pCF->ExpData->PutData(pCF->Data,ConstParams::DATA_NUM); // データ格納
    30 :	}
    31 :	if(pCF->CmdFlag==CTRL_EXIT){
    32 :	    // 終了処理モード (ここは制御終了時に1度だけ呼び出される(非実時間空間なので重い処理もOK))
    33 :	    delete PlantSim1; PlantSim1 = nullptr;
    34 :	}

＜シミュレーション結果＞

黒の実線がMATLABによるシミュレーション結果，赤の破線がARCSによる計算結果。完璧に一致。

上から順に (a) q軸電流指令，(b) モータ側速度，(c) 負荷側速度

ということで，クラスの初期化時に自動的に離散化されるようになり，これからはワザワザMATLABでc2d関数使って一々離散化しなくて良い。すばらしい！

11. 何かおかしいと思ったら。。。

計算結果がなーんか変やな～と思ったら，メールを下さい。時間があれば対応します。
内部計算で逆行列を計算しているので，そのdetが極端に零に近い場合は，計算結果がおかしくなります。そういうときは，元のA行列がたぶんなんか変な行列になってます。

ライセンス/Licenses

Copyright (C) 2011-2015 Yuki YOKOKURA
This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 3 of the License, or any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details . Besides, you can negotiate about other options of licenses instead of GPL. If you would like to get other licenses, please contact us.