モータ角度制御

モータドライバを使ったサーボモータ制御装置です。

ソースコードやプロジェクトファイルのダウンロードはこちら →motor.zip

（１）シミュレーション用のプログラムです。

ブロック図

プログラム
proc { time = currTime(); // 現在時刻 Pc = amp * (time>1) * (time<4); // 位置指令（1～4秒以外は0） err = Pc - pos; // 誤差 acc = Kp * err - Vc; // 加速度 acc = limit( -Amax, Amax, acc ); // 加速度制限 Vc = integral( acc, Vc, T, Vmax, -Vmax ); // 速度指令 pos = integral( Vc, pos, 1, 10, -10 ); // 位置計算(シミュレーション用) }

説明
モータドライバには速度サーボが内蔵されていて、速度指令を与えればモータの回転速度を自由に変化させることができます。しかし、位置（回転角度）を制御するには、位置指令から速度指令を生成する必要がありますので、その機能を Aproc-1 のプログラムで実現します。

この例では、積分器とリミッタを組合わせて速度指令の軌道を生成していますので、どのような指令に対してもモータに激しい加速度が生じないようになっています。

上記プログラムでは、以下の関数、信号、およびパラメータを使っています。

関数　currTime 運転開始時からの時間[単位秒]を得ます。

　limit リミッタ（飽和器）です。

　integral 出力リミット付き積分器です。

信号　time 運転開始から数えた経過時間（秒）です。

　Pc 位置指令信号です。

　Vc 速度指令信号です。実際の速度も同じと仮定しています。

　err 誤差信号です。

　acc 加速度に相当する信号です。単位は異なります。

　pos 速度（指令）を積分して得た位置です。

パラメータ　amp 指令振幅です。

　Kp 位置制御ゲインです。

　T 速度指令を生成する積分の時定数です。

　Vmax 速度の最大値です。積分器を飽和させます。

　Amax 加速度を制限するリミット値です。

設定例

シミュレー
ション例

（２）Aproc-1 Plus を使った実運転用のプログラムです。

ブロック図

プログラム
青字は変更点
proc { Pc = amp * ( (time>5) && (time<25) ); // 5～25秒では Pc=amp それ以外では0 pos = Cin( 0, 0.01/4000 ); // 0ch より位置情報の検出 err = Pc - pos; // 誤差 acc = Kp * ( Pc - pos ) - Vc; // 加速度（相当）の計算 acc = limit( -Amsx, Amax, acc ); // 加速度制限 Vc = integral( acc, Vc, T, Vmax, -Vmax ); // 速度指令 Aout( 0, 0, scaleOUT, Vc ); // モータドライバへ速度指令を出力 }

説明
実際にモータを運転するには、モータの回転角度を計測するための位置センサが必要です。 Aproc-1 Plusには、4チャンネルのエンコーダパルスカウント機能があり、関数 Cin を使ってその値を入力できます。また、速度指令をサーボドライバへアナログ電圧で与えるために、関数 Aout を使います。

　Cin エンコーダパルスを積算したデータを入力します。

　Aout アナログ電圧を出力します。

ここでは、ボールねじ（ねじピッチ 10[mm/rev]）の回転駆動によりステージの位置を制御することを考えます。エンコーダのパルス数は4倍加算されますので、エンコーダのパルス数を 1000[p/転] とすれば、 1パルスの増加で移動するボールねじの変位は、0.01[m/転]÷4000[p/転] = 0.01/4000[m/p]です。よって、関数 Cin のスケールとして 0.01/4000 と与えれば、信号 pos の単位は[m]になります。

次に、使用するモータドライバはアナログ電圧で速度指令を与えるもので、そのスケールは 1200[rpm/V] とします。 1200[rpm/V] = 20[rps/V] = 0.2[m/s/V] なので、関数 Aout のスケールを 0.2[m/s/V] と設定すれば信号 Vc の単位は [m/s] になります。以上の設定により、シミュレーション(1)の結果と同じ特性を実機で実現できます。
動作が確認できたら、位置指令Pcを目的とする動きとして与えることにより目的を達成できます。マニュアルで不規則な指令を与えたい場合は、操作パネルのトラックバー、またはアナログ入力から与えることもできます。

（3） 3台のモータを制御するシミュレーション用プログラムです

プログラム
// 3台のモータによる位置制御(シミュレーション) motorS( pc, gain, T, vx, ax ) //ユーザ関数 { err = pc - pos; // 誤差 acc = gain * err - vc; // 加速度の計算 acc = limit( -ax, ax, acc ); // 加速度制限 Vc = integral( acc, Vc, T,vx, -vx ); // 速度指令 pos = integral( Vc, pos, 1, 10, -10 ); // 位置計算（シミュレーション) return pos; } proc //メイン処理 { time = currTime(); // 現在時刻 pcX = (time>1) * (time<16) * amp; // 位置指令(X) posX = motorS( pcX, Kp, T, Vmax, Amax ); // 位置(X) vcX = auxfunc( Vc ); // 速度指令(X) pcY = (time>5) * (time<20) * amp; // 位置指令(Y) posY = motorS( pcY, Kp, T, Vmax, Amax ); // 位置(Y) vcY = auxfunc( Vc ); // 速度指令(Y) pcZ = (time>9) * (time<24) * amp; // 位置指令(Z) posZ = motorS( pcZ, Kp, T, Vmax, Amax ); // 位置(Z) vcZ = auxfunc( Vc ); // 速度指令(Z) }

説明
プログラム(1)の制御部分をユーザ関数 motorSim とし、これを3回呼び出しています。それぞれの添え字をX、Y、Zとし、X軸は時刻1～16秒に、Y軸は時刻5～20秒に、Z軸は時刻9～24秒にステップ状の指令を与えました。制御のパラメータは３軸とも共通です。

記録例

（3） 3台のモータを制御するシミュレーション用プログラムです
プログラム	// 3台のモータによる位置制御(シミュレーション) motorS( pc, gain, T, vx, ax ) //ユーザ関数 { err = pc - pos; // 誤差 acc = gain * err - vc; // 加速度の計算 acc = limit( -ax, ax, acc ); // 加速度制限 Vc = integral( acc, Vc, T,vx, -vx ); // 速度指令 pos = integral( Vc, pos, 1, 10, -10 ); // 位置計算（シミュレーション) return pos; } proc //メイン処理 { time = currTime(); // 現在時刻 pcX = (time>1) * (time<16) * amp; // 位置指令(X) posX = motorS( pcX, Kp, T, Vmax, Amax ); // 位置(X) vcX = auxfunc( Vc ); // 速度指令(X) pcY = (time>5) * (time<20) * amp; // 位置指令(Y) posY = motorS( pcY, Kp, T, Vmax, Amax ); // 位置(Y) vcY = auxfunc( Vc ); // 速度指令(Y) pcZ = (time>9) * (time<24) * amp; // 位置指令(Z) posZ = motorS( pcZ, Kp, T, Vmax, Amax ); // 位置(Z) vcZ = auxfunc( Vc ); // 速度指令(Z) }
説明	プログラム(1)の制御部分をユーザ関数 motorSim とし、これを3回呼び出しています。それぞれの添え字をX、Y、Zとし、X軸は時刻1～16秒に、Y軸は時刻5～20秒に、Z軸は時刻9～24秒にステップ状の指令を与えました。制御のパラメータは３軸とも共通です。
記録例

（4）Aproc-1 Plus で3台のモータを制御するプログラムです

プログラム
motor( ch, pc, gain, T, vx, ax, scale ) // ユーザ関数 { pos = Cin( ch, scale ); // エンコーダ読出し err = pc - pos; // 誤差 acc = gain * err - vc; // 加速度（相当）の計算 acc = limit( -ax, ax, acc ); // 加速度制限 vc = integral( acc, vc, T, vx, -vx ); // 速度指令 Aout( ch, 0, 0.2, vc ); // モータドライバへ速度指令を出力 return pos; } proc // メイン処理 { time = currTime(); // 現在時刻 if time <= 0 { scl = 0; } // 初回のみリセット else { scl = 0.01/4000; } pcX = (time>1) * (time<16) * amp; // 位置指令(X) posx = motor( 0, pcX, Kp, T, Vmax, Amax, scl ); // 変位(X) vcX = auxfunc( Vc ); // 速度指令(X) vcY = auxfunc( Vc ); // 速度指令(Y) posy = motor( 1, pcy, Kp, T, Vmax, Amax, scl ); // 変位(Y) pcY = (time>5) * (time<20) * amp; // 位置指令(Y) pcZ = (time>9) * (time<24) * amp; // 位置指令(Z) posz = motor( 2, pcz, Kp, T, Vmax, Amax, scl ); // 変位(Z) vcZ = auxfunc( Vc ); // 速度指令(Z) }

説明
(3)のシミュレーションプログラムに角度入力関数 Cin と速度指令出力関数 Aout を追加し、実際に3台のモータを制御します。

ユーザ関数のパラメータに、チャンネル番号 ch とエンコーダスケール scale を追加して入出力に使っています。また、メイン処理では、運転開始時のみエンコーダスケールを 0 として、パルスカウントを初期化しました。

（4）Aproc-1 Plus で3台のモータを制御するプログラムです
プログラム	motor( ch, pc, gain, T, vx, ax, scale ) // ユーザ関数 { pos = Cin( ch, scale ); // エンコーダ読出し err = pc - pos; // 誤差 acc = gain * err - vc; // 加速度（相当）の計算 acc = limit( -ax, ax, acc ); // 加速度制限 vc = integral( acc, vc, T, vx, -vx ); // 速度指令 Aout( ch, 0, 0.2, vc ); // モータドライバへ速度指令を出力 return pos; } proc // メイン処理 { time = currTime(); // 現在時刻 if time <= 0 { scl = 0; } // 初回のみリセット else { scl = 0.01/4000; } pcX = (time>1) * (time<16) * amp; // 位置指令(X) posx = motor( 0, pcX, Kp, T, Vmax, Amax, scl ); // 変位(X) vcX = auxfunc( Vc ); // 速度指令(X) vcY = auxfunc( Vc ); // 速度指令(Y) posy = motor( 1, pcy, Kp, T, Vmax, Amax, scl ); // 変位(Y) pcY = (time>5) * (time<20) * amp; // 位置指令(Y) pcZ = (time>9) * (time<24) * amp; // 位置指令(Z) posz = motor( 2, pcz, Kp, T, Vmax, Amax, scl ); // 変位(Z) vcZ = auxfunc( Vc ); // 速度指令(Z) }
説明	(3)のシミュレーションプログラムに角度入力関数 Cin と速度指令出力関数 Aout を追加し、実際に3台のモータを制御します。ユーザ関数のパラメータに、チャンネル番号 ch とエンコーダスケール scale を追加して入出力に使っています。また、メイン処理では、運転開始時のみエンコーダスケールを 0 として、パルスカウントを初期化しました。

更新：2019年9月27日