C＋＋言語でモータをシミュレーションする

　MatLabとかPythonで既存のライブラリを用いてシミュレーションしたことがあると思います。
でも，その中身はどうなっているのかわからないで使っていることも多いと思います。
実験ではリアルタイム制御を用いていることが多く，シミュレーションと実験での誤差が大きくなることがあります。
そこで今回はC言語を用いて，リアルタイム制御する実験環境を再現するシミュレーションを行いたいと思います。
 

まず制御とは？

ここで制御について簡単に説明します。制御はある目的の値（参照値）が出力の値と一致していることが目的です。
いろんな制御がありますが，どの制御もやっていることは一緒です。
位置を制御したかったら自分の思った位置に瞬間で出力が同じ位置にあることが望まれます。
しかし，現状はそんなことはなく，外力やノイズの影響により機械が暴走したりすることがあります。
機械が暴走（不安定）にならないように安定になるように全体の設計を考えるというものが制御です。

具体的にどう考えるか？

制御には，まずコントローラとプラントがあります
　まず，プラントとは何を動かすかという「もの」です。
具体的には，モータ，車，エアコン，エアコンの空気，熱，流体，電流，電圧，化学反応系，人間社会システムなど様々です。
制御ではモノに入力を与え，そのモノの挙動を予測および観測をしてどのような入力を与えればよいかを逐次的に考えます。
モノが変われば挙動もかわるのでプラントがどのような物理特性をもっているのか，というモデリングが重要になります。
　つぎにコントローラですが，これは目的の値（所望位置や所望速度，所望トルク）があらかじめ設定してあって（これを考えるのは機械学習や経路計画などの上層の軌道設計にあたる），その値に沿って出力の値が時間が遅れることなく一緒になるようにするには，どのような入力をプラントに与えればいいのかを考える。というのがコントローラです。
これは端的に言えば，コントローラはプラントへの入力までしか制御できないということなので，プラントの中身を変えるということはできないことになります。
　実験では，コントローラで計算した入力をプラントに印加し，出てきた値を読み取るだけなのでプラントのモデリングは必要なくても制御できることがあります。しかしシミュレーションではどのようにものが動くかを考えなければならないのでモノのモデリングが大事です

モデリング（プラント）

今回は単純にサーボモータの角度を制御することを考えます。

モータは電流を印加するとトルク定数に応じたトルクを発生させます。
発生したトルクによりモータが回転します。回転した際に機構部の摩擦により粘性摩擦力と静止摩擦力が生じます。
今回は静止摩擦力を無視しています。また，駆動させたときに他の物体との接触による外力やトルクリプルによる変動等の影響を受けます。
そのため，モデリングとしてはモータに粘性摩擦と外力が加わる系を考えます。

J_m \ddot{\theta}_m = K_t I_m - D_m \dot{\theta}_m - T_{ext} 

 ここで，

J_m, \theta_m, K_t, I_m D_m, T_{ext}

はモータの慣性モーメント，角度，トルク定数，粘性摩擦係数，外力トルクです。

　ここで制御工学では微分方程式で表された運動方程式は解析しずらいため，ラプラス演算子というものを用います。ラプラス演算子を用いると微分を乗算にして考えることができるため，取り扱いが楽になります。

ラプラス演算子を用いる運動方程式は

 J_m s^2 \theta_m = K_t I_m - D_m s \theta_m - T_{ext}

となります。ラプラス演算子のsは微分を表しており，sの２乗が2回微分を表していることがわかると思います

制御系

モデリングができたら，モータの角度を制御する制御系を考えます。
今回は簡単のために制御で一般的なProportional Derivative (PD)制御を用いて制御します。
　ここで先ほどのラプラス演算子で用いた運動方程式を用いて，ブロック線図を描きます。
ブロック線図とは信号の流れがどうなっているのかを記述するためのものです。

所望の位置(command)と位置の応答値(response)の位置偏差に対してゲインをかける比例ゲインKpを用い，速度の偏差に対してゲインをかける微分ゲインKdを用います。これらの足し合わせにより加速度参照値(reference)を生成します。加速度による制御はモーションコントロールにおける重要な要素であり，位置だけでなく力，速度も同様にして考えることができるというものが特徴です。
今回は指令値の速度を０としています。　

実装

制御系が設計できたら，最後にシミュレーションです。
指令値は0.1 radとし，外力は0.01 Nとしました。慣性と粘性摩擦係数は適当な値を用いています。
制御周期はリアルタイム制御を考慮して100 μsとしました。
プラントの計算ではその100倍の速さ(1μs)で計算をしています（実世界では連続時間なのでサンプル間隔は無限小です）。
C＋＋言語で記述して出力はDATファイルとしました。
描画はgnuplotを用いています。

#include <bits/stdc++.h>
using namespace std;
#include <stdio.h>
#include <math.h>
#define FILE_NAME "PDtest.dat"

int main(void){
    double t=0.0;
    const double dt= 0.0001;
    const double Jm= 0.00040;
    const double D = 0.0040;
    const double Kt= 0.156;
    const double Kp= 10000;
    const double Kv= 100;
    double thcmd   = 0.0;
    double dthcmd  = 0.0;
    double thres   = 0.0;
    double Tref    = 0.0;
    double Tres    = 0.0;
    double Imref   = 0.0;
    double dthres  = 0.0;
    double ddthres = 0.0;
    double ddthref = 0.0;
    double Text    = 0.01;

    FILE *fp;
    fp=fopen("PDtest.dat","w");
    for(t=0;t<=3.0;t+=dt){
    //Trajectory Planning
    thcmd=0.10;//*sin(2.0*t);
    //Controller
    ddthref=Kp*(thcmd-thres)-Kv*dthres;
    //Tm_ref
    Tref=Jm*ddthref;
    //Im_ref
    Imref=Tref/Kt;
//Plant
    //T
    Tres=Kt*Imref;
    for(int p=0; p<100; p++){
        //ddthres
        ddthres=(Tres-D*dthres-Text)/Jm;            
        //thres
        thres +=dthres*(dt/100);
        //dthres
        dthres+=ddthres*(dt/100);
    }
    fprintf(fp,"%f %f %f %f %f \n", t,thcmd ,thres,dthcmd , dthres);
    }
    printf("ok!\n");
    fclose(fp);
    printf("%sを作成しました。\n",FILE_NAME);
    return 0;
}

結果

生成されたPDtest.datファイルをgnuplotを用いて描画します。
gnuplotでは事前にpltファイルを作成しておくと描画の設定を使いまわせるので便利です。
今回用いたpltファイルを示します。

#データファイル名指定
name1 = "PDtest"

#datファイルにする
data1= sprintf("%s.dat",name1)

#出力epsファイル名指定(epsで使用)
epsname = "PDtest.eps"

#epsサイズの変更
epswidth = 6
epsheight= 4

#線のスタイル指定，lt:line type, linestyle(ls)で使用
set style line 1 lc rgb "# c80000" lw 2 #red
set style line 2 lc rgb "# 00c800" lw 2 #green
set style line 3 lc rgb "# 0000c8" lw 2 #blue
set style line 4 lc rgb "# 0000c8" lw 2 #blue

#軸の設定
set tics font "TimesNewRomanPSMT,12"

#x軸小目盛の設定，数値指定で目盛幅の変更
set mxtics
set xlabel font "TimesNewRomanPSMT,14"
set ylabel font "TimesNewRomanPSMT,14"

# %(n.m)f:小数点以下m桁，桁数(小数点や符号含む)nで表示
#set format y "%3.2f"
#set format x "%2.1f"

#描画範囲の設定
#set xrange [0.0:0.5]
#set yrange [-1.0:3.0]

#ラベル設定
set xlabel "Time [t]"
set ylabel "Position [rad]

#凡例の設定
set key horizontal 
set key samplen 1
set key font " TimesNewRomanPSMT,10"

#描画＃
# xres #
plot data1 u 1:2 title "X_{cmd}" with lines ls 1, data1 u 1:3 title "X_{res}" with lines ls 2 

位置制御のシミュレーション結果は以下の通り

外乱の影響と積分器がないため，定常偏差が残っていることがわかります。

まとめ

いかがでしょうか。
実際は離散系で考えなければならなかったりするのですが，今回は連続時間でやってみました。
流れを踏めば意外と難しくないので，ぜひ実装してみてください。