ぽみぽみの備忘録

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

　今回は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$ は2回微分を表していることがわかると思います

制御系

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

所望の位置(command)と位置の応答値(response)の位置偏差に対してゲインをかける比例ゲイン $K_p$ を用い，速度の偏差に対してゲインをかける微分ゲイン $K_d$ を用います。これらの足し合わせにより加速度参照値(reference)を生成します。今回は指令値の速度を０としています。　

実装

制御系が設計できたら，最後にシミュレーションです。
指令値は0.1 radとし，外力は0.01 Nとしました。慣性と粘性摩擦係数は適当な値を用いています。
制御周期はリアルタイム制御を考慮して100 μsとしました。
プラントの計算ではその100倍の速さ(1μs)でオイラー法で計算をしています（実世界では連続時間なのでサンプル間隔は無限小です）。
より実プラントに近いシミュレーションをするには固定ステップのRK4（ルンゲクッタ）や可変ステップを行うのが通常です。
時系列出力は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ファイルを示します。
ステップ外乱に対して，制御器が1次以上の積分器を持っていないため，定常偏差が生まれています。

#データファイル名指定
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

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

まとめ

実際はプラントは連続時間です。今回はプラントの実装をオイラー法でやってみました。

ぽみぽみの備忘録

モーションコントロールとかメカトロとかの備忘録

モータ位置制御をC言語でシミュレーション