X,Y,Z3軸傾斜角をはじめから求めようとするとかなり複雑のため、この記事では1軸分(Y軸)まわりの傾斜角を加速度センサおよびジャイロセンサから算出する方法を解説します。

MPU-6050では下図の向きにX、Y、Z軸を設定しています。

加速度センサでY軸まわりにθ_y傾斜する場合はZ軸方向およびX軸方向の重力比率より傾斜角θ_yは逆三角関数を使用すると算出できます。

加速度センサアプリでも説明しましたが、加速度センサの重力(1G)成分による各軸の値a_x,a_y,a_zは重力に対する抗力の向きとなりますので、下図のようにY軸中心に座標を正方向θ_Y傾斜したとき、重力Z軸成分(1G_Z)の抗力a_zは正値で、X軸成分(1G_X)の抗力a_xは負値となります。

ここでは動作による加速度は考えていませんので算出データは地上に対して傾斜させて静止した角度(絶対角度)となります。

算出した絶対角度にオフセットθ_offset分を差し引くと、任意の角度で基準0°にプリセットできるので基準からの相対角度とすることができます。逆三角関数を使用した場合の角度は単位がradのため、必要に応じて°に変換します。

ジャイロセンサの場合、データg_yはY軸まわりの回転時の角速度のため、角度を得るにはプログラム内で積分して算出する必要があります。

まず得られた16ビットデータをスケールに応じた角速度（°/s）に変換します。それからオフセットがある場合は予め差し引いておいてから積分して角度を算出します。

プログラム内での積分は離散化した近似です。最もシンプルな積分は下図のアルゴリズムです。サンプリングタイムTsが十分小さい場合はこれでも問題はありません。

MPU-6050をNucleo-F103RBに接続して使用する場合の配線例を下図に示します。

センサボードGY-521の電源にはLDOレギュレータを内蔵していますので5Vを接続します。

データ出力インターフェースはI²C通信のため、STM32側ではI²C2に接続します。I²CのI/Oはオープンドレインに設定しますので通常ではプルアップ抵抗が必要ですが、センサボードGY-521には予め含まれています。

まず、はじめの関門はI²C通信の設定およびデータ読み書きを正常に実現することです。データを任意に安定して読み込めることができればあとはこれまでに解説した方法でデータを演算して角度を算出するだけです。

I²C通信デバイスを使用するために、I2C2_Configuration()でまず初期設定をします。これは他のI²Cデバイスと同様です。詳細はシリアル通信I2C【STM32のI2C詳細】を参照してください。

I2C2の初期設定のあとはMPU-6050の設定をします。設定はレジスタに値を書き込みます。すべてデフォルトで使用する場合は必要ありませんが、デバイスに内蔵のデジタルフィルタを使用したり、データのフルスケールを変更設定する場合には必要です。

I²C通信では1バイト単位でデータを読み書きするのですが、データ読み込みでは個々のバイトデータを単独で読み込むのではなく、一度に複数バイトを読み込む方が効率的です。

そうなると読み込みデータアドレスは加速度センサのX軸のデータアドレス3Bから14バイト分を読み込んでデータを配列に格納するだけでよいのです。

話は前後しますが、所望のレジスタに1バイトデータを書き込むための関数は自作します。

ここでのポイントはスレーブアドレスで、I²Cのアドレス送信関数I2C_Send7bitAddress()ではアドレスは1バイト枠で左詰め7ビット分です。これに最下位ビットに送信か受信フラグを付加した１バイトデータとして扱います。

ライブラリ関数の中身を確認して動作をたどると理解できると思います。

例えばI²Cデバイスのスレーブアドレスが0x68の場合は1ビット分左シフトした0xD0を送信関数I2C_Send7bitAddress()に与えるアドレスとします。

データ読み込みは書き込みの延長のようなものですが、複数バイトデータも連続して読み込むことができます。

これも自作の複数バイト読み込み関数ですが、データが複数ある場合は最後のバイトデータ受信直前でACKを返さないNACKとすることがポイントです。

MPU-6050 は初期設定としてレジスタへセンサへのクロック源を設定したり、加速度センサ、ジャイロセンサのフルスケールや出力へのフィルタを設定します。

加速度センサおよびジャイロセンサにより角度を取得するだけならばデフォルト設定でも使えますが取扱うレジスタはせいぜい下記の程度です。

クロック源を指定することで MPU-6050は機能始めます。クロック源はレジスタアドレス6Bの下位3ビット分に設定します。電源投入時のデフォルトでは内部発振8MHzとなっています。

MPU-6050には出力にノイズ対策のローパスフィルタを設定することができます。必要に応じてレジスタアドレス1Aの下位3ビットに0から6までの値を設定します。

レジスタアドレス1Bではジャイロセンサフルスケールを0から3までの値で設定します。デフォルトでは±250°/sです。

レジスタアドレス1Cでは加速度センサフルスケールを0から3までの値で設定します。デフォルトでは±2gです。

加速度センサの各軸データは16ビットですが、データ格納アドレスは8ビットですので上位8ビット、下位8ビットで構成されます。X軸、Y軸、Z軸のデータはレジスタアドレス3BよりX軸上位、下位...と順に格納されます。

データについては例えばフルスケールが±2gの場合は、+2g時に32768となるので1g当たり16384となります。

ジャイロセンサの各軸データも加速度センサと同様にX軸、Y軸、Z軸のデータはレジスタアドレス43よりX軸上位、下位...と順に格納されます。

データについては例えばフルスケールが±250°/sの場合は、+250°/s 時に32768となるので1°/s当たり131となります。

加速度およびジャイロセンサからのデータが取得できるようになるといよいよそれぞれのデータから角度を算出できるようになります。

I2C_Read_multibyte()関数による複数バイト読み込みで格納したデータにより加速度およびジャイロセンサ各軸の16ビット値が取得できます。

取得できた値により加速度センサによる角度およびジャイロセンサによる角度が算出できます。

ジャイロセンサからの角速度データを積分して角度を算出するためのサンプリングタイムを正確に5msとするためにRTOSの機能を利用しています。詳細はFreeRTOSタスク管理の基本を参照してください。

加速度センサおよびジャイロセンサから単独で算出された角度はそれぞれ一長一短あり安定しません。そこで、両センサの長所である部分をいいとこ取りするための手段に相補フィルタというものを使います。

加速度センサでは重力変化による傾斜角度が得られるので特に静止状態の場合の絶対的な角度変化は信頼性がありますが、情報にノイズが含まれていたり、動作が伴うと重力以外の成分も加わることにもなります。

対して、ジャイロセンサでは回転を検知した角速度が得られるので動作の伴う角度の変化には信頼性がありますが、プログラム内で積分をしているので誤差も同時に累積しドリフトを発生してしまうことになります。

そこで相補フィルタを使用して、加速度センサ出力にはローパルフィルタ(LPF)により低周波部分だけをとりだし、ジャイロセンサ出力にはハイパスフィルタ(HPF)により、ドリフト分をキャンセルすることができます。

相補フィルタの式だけをみると、至ってシンプルなのですが、奥が深いので別途、相補フィルタのしくみを解明してみる【加速度・ジャイロセンサ】で詳細を解説しています。

下図は加速度センサ、ジャイロセンサおよび相補フィルタを通した出力による角度変化の様子をグラフ化したものです。横軸は経過時間(s)、縦軸は角度(°)です。

加速度センサによる角度は細かなノイズが含まれていますが絶対的な角度を示すのでドリフトは発生していません。

ジャイロセンサによる角度はプログラム内で積分したものなのでノイズもなく滑らかですが、累積誤差によるドリフトが発生しています。

相補フィルタを通した出力では加速度センサによる出力とジャイロセンサによる出力の長所のみを引き継いだものになっていて安定して信頼性があります。相補フィルタのカットオフ周波数は2Hzに設定したものです。

相補フィルタのカットオフ周波数によりどのような違いがあるのかを確認するために4Hzと2Hzで比べてみました。

カットオフ周波数を4Hzに設定した場合はジャイロセンサによるドリフトの影響はないのですが、加速度センサの敏感なノイズ成分が多少含まれています。加速度センサ寄りの影響を受けた出力になっています。

カットオフ周波数を2Hzに設定した場合は加速度センサのノイズ成分はほぼキャンセルされ、ジャイロセンサ寄りの影響を受けた出力になっています。ジャイロセンサ出力からドリフト分だけをキャンセルした出力となっています。

下の図はカットオフ周波数を2Hzの場合で、少し速い動作をさせてみた出力結果です。

加速度センサでは速い動きほどノイズ成分が顕著に現れ、ジャイロセンサではドリフト成分によるオフセットが突然大きくなったりでこのままでは使用できません。

対して、相補フィルタを通した出力ではこのような動作でも遅れは見られずしっかりと追従し、ドリフトの影響もなくほぼ理想の状態となっていてフィルタ効果が確認できます。

相補フィルタをブロック線図で表すと下図のようになります。

加速度センサではノイズ成分を除去した低周波領域の情報がほしいためにローパスフィルタ(LPF)を通します。

ジャイロセンサでは累積するドリフト成分を除去するためにハイパスフィルタ（HPF)を通すと情報のドリフトによるオフセット分をキャンセルできます。

加速度センサあるいはジャイロセンサ単独にそれぞれフィルタを通すだけでなく、組み合わせることでいわゆる互いにいいとこ取りしあうところから相補フィルタと呼ばれます。

相補フィルタに使用するLPFとHPFには条件があり、任意の周波数でフィルタゲインの和が1であることです。

相補フィルタのLPFとHPFでカットオフ周波数を2Hzとした場合の特性を周波数領域のボード線図で表すと下図のようになります。縦軸はゲインを表していて単位はdB(デシベル）ですが、この条件ではLPFとHPFのゲインは足すと全領域で1となります。

LPFはカットオフ周波数fc以上の高周波成分を除去しますので加速度センサからの情報は低周波部分のみが残ります。

HPFはカットオフ周波数fc以下の低周波成分を除去しますのでジャイロセンサからの情報はゆっくりしたドリフトによるオフセット分が除去されます。

カットオフ周波数fcが2Hzとした場合はほぼジャイロセンサによる情報が優勢になります。最終的には出力を見ながらfcを微調整をすればよいと思います。

相補フィルタは結果の式だけ知っていれば使用はできるのですが、式の意味を知らなければなんとなくの当てずっぽうで係数等のパラメータ値を決めることになります。

相補フィルタを使用するのであればやはりある程度式の内容は理解しておきたいものです。

以下の算出過程には難解な理論は使用していないのですが理解するには最低限の古典制御理論の基礎知識が必要ですので、当サイトのフィードバック制御のための基礎知識【準備編】【解析編】を参照してください。

ここでは1次遅れフィルタ、ラプラス変換などの概念がわかっていれば理解できます。

ようやくよく目にする相補フィルタの式を導きました。これは１次フィルタの場合です。これで重み係数Kの意味もわかると思います。

係数Kはカットオフ周波数fcとサンプリングタイムT_S で決まるものです。

一般的に出回っている相補フィルタ情報の中には加速度センサとジャイロセンサの出力に係数Kを重みとしてかけた解釈のものもありますが、これでは意味をなしません。

一般的なモータの位置決めサーボシステムのブロック線図です。パワーアンプが電流指令タイプの場合は一番内側に電流フィードバックループがあり、モータへは電流指令を与えます。電圧指令タイプの場合は電圧指令値をオープン制御で与えることになります。

モータ位置決めサーボシステムではまず速度制御フィードバックによりモータ速度を安定させてから、外側に位置フィードバックループを設けるものが一般的です。

速度制御にはマイコンプログラムで簡単に実現でき、外乱抑制にも効果のあるハイゲインフィードバック方式を採用します。

位置制御システムに発展させるためには速度制御系の外側に位置フィードバックループを設けます。

ハイゲインフィードバック方式では速度制御系は1次遅れ系に近似できます。位置制御の補償器が比例ゲインK_Pだけなのは入力から出力までの伝達関数を2次遅れ系とするためです。

外部から任意のパルスを与える場合は多少難がありますが、あらかじめ位置目標値の軌道がわかっている場合は速度指令値が作成できるのでフィードフォワード項として付加すると遅れはほぼなくなり追従性はより向上します。

入力から出力間の伝達関数は以下のように2次遅れ系となります。時定数T_m2は既知のため、減衰比ζのみを指定するとゲインK_Pは一意的に決まります。

例えば 時定数T_m2が25msの場合は減衰比ζを1に指定するとω_nが20となるのでK_Pは10になります。

モータの位置および 外部入力パルスによる位置指令値はタイマのエンコーダインターフェースモードによりアップダウンカウントすることで得られます。

User SWは位置情報をリセット/プリセットするために使用します。リセットしたときにアップダウンカウンタの中間値あたりに指定しておけば、動作中にカウンタのオーバーフローやアンダーフローによる動作不良を防ぐことができます。

まず恒例のシミュレーションをマイクロソフトExcelで実施しました。条件としてハイゲインフィードバックゲインC1,C2をそれぞれ2,3とする場合で検証します。

入出力間伝達関数は２次遅れ系となりますので減衰比ζを1と指定すると位置ゲインK_Pは15となります。

この条件では２次遅れ系の特徴がよく現れていて、入力の指令値がランプ状で変化しているときは、出力はわずかな定常偏差をもって変化し、定値になったところで定常偏差がなくなるサーボ追従をしています。制御理論の最終値定理のとおりです。

ハイゲインフィードバックゲインC1,Ｃ2を上げすぎると速度の追従性および外乱抑制は向上しますが、敏感になりすぎて振動的になります。位置サーボの場合は、小さめのゲインでも十分効果はあります。

実機による動作の実測値です。エンコーダ軸を手動で回すと軸の動きに合わせて発生するパルスに追従するいわゆるパルス同期運転となります。

正転、逆転ともにモータの位置は任意に与えたパルスの通り、つまりエンコーダの軸の動きに追従しているのが確認できます。

多少複雑な入力パターンの場合でもしっかり追従しています。

シリアルモニターでエンコーダからの位置指令パルス(上段)とモータ位置パルス(下段)の値を確認してみますと、モータ位置パルスは指令パルスに追従しているのがわかります。

速度制御系でハイゲインフィードバック方式や2自由度ロバスト制御が適用されて指令値通りに応答できるようになっていれば、位置決めサーボ系に発展させることは容易です。

ここでは位置決めサーボ系において応答を決めるゲインをほぼ一意的に選定できる加速度指令方式を採用して実機に適用してその有効性を検証していきます。加速度指令方式についてはマイコンで実現するフィードバック制御のための基礎知識【発展編】で詳細を解説しています。

この方式では設定する目標軌道の位置のみならず、速度および加速度に分解したものを予め作成して指令値として与えます。

ゲインKp, Kvの決め方はマイコンで実現するフィードバック制御のための基礎知識【解析編】で解説している2次遅れ特性を参考にしてください。ゲインを適切に設定することにより位置偏差がゼロに収束していわゆる位置決めサーボが実現できます。

上式が成立するのは速度系にロバスト制御が施されていて速度指令値θ^'ref ≒ 速度θ^' となることにより加速度指令値θ^"ref ≒ 加速度θ^" とみなせるからです。

適切に設定したゲインKv、Kpによって起動時に発生している誤差が２次遅れ系の特性で収束すると位置θは目標値 θ₀に遅れなしに追従することになります。詳細はマイコンで実現するフィードバック制御のための基礎知識【発展編】の位置決め追従制御(加速度指令方式)で解説しています。

今回は速度制御系の部分にはハイゲインフィードバック方式を採用して簡易的なロバスト制御を実現しています。

指令値は台形速度パターンが最も簡単なので今回はこれを採用しています。プログラムで浮動小数点を使用すればsin関数を用いた加速度曲線などが定義でき、よりスムーズなパターンが作成できます。

指令値を作成するにあたってはマイクロソフトExcelを使用してサイクルタイム毎に刻んで演算すれば下図のような数値が得られます。

実際のプログラミングではExcelの演算と同じものをそれぞれ加速度、速度および位置指令値の配列に代入すればよいわけです。準備する配列の個数は下図の例ではサイクルタイムを5msとしているため各1000個ずつ必要です(5s/5ms個)。

フィードバックおよび実機計測に取り込む速度と位置のデータはエンコーダからのパルスをNucleoに取り組みますが、速度と位置はそれぞれ別個のタイマ・カウンタで計測します。

外乱負荷を与えない状態での速度変化の指令値と実測値です。起動時はどうしてもわずかに乱れていますがハイゲインフィードバックの影響かもしれません。エンコーダ分解能が極めて粗いのと、演算を整数化しているため、一工夫しているとはいえ特に低速時には理想通りには動作させるにはあと何か必要かもしれません。

位置決めサーボでの速度実測値ですので速度定常時にわずかに脈動しているのは問題ではありません。

0.5s毎に外乱負荷を与えた場合の速度です。わずかに外乱の影響はありますが、ハイゲインフィードバックが効いてほぼ安定しています。

実際のモータの移動位置です。位置指令値と実測値にはずれが見られず遅れなく追従しているのが確認できます。外乱負荷を与えた場合でも位置にはほぼ影響がありません。

レゴEV3Lモータは一回転あたり180パルスのエンコーダを内蔵しています。この指令値はちょうど7回転で最終到達パルスが1260パルスになるものです。

レゴEV3Lモータのような簡易的なエンコーダがついたものでも高度なロバスト制御を施した位置決めサーボに発展させると高精度な位置の追従制御ができることが確認できました。

ハードウェアはマイコンと電圧制御タイプのドライバのごく普通の組み合わせのものなのですが、本格的な理論を適用したモーションコントロールが簡単に実現できてしまうのです。

制御理論の応用も特殊なハードウェアを必要とするものでなくだれでも身近に応用できるものを紹介しました。ここではモータを使ったモーションコントロールを例にあげて解説してきましたが、それ以外の用途に活用しても面白いと思います。

オープン制御の場合は、速度指令値を台形パターンにするにはその指令値に比例した電圧となるようなPWMデューティ比率に指定します。

モータ速度は無負荷の状態では与えた電圧に比例して動作するはずです。

実際の運転で起こり得る負荷を想定してモータ軸にブレーキがかかる向きに約0.5s間隔で一定の負荷トルクを与えて出力のモータ速度にどのような影響がでるかを調べてみます。

マイコンで実現するフィードバック制御のための基礎知識【応用編】で解説しましたが、DCモータはオープン制御の場合は負荷がかかると、大きく影響を受けてしまいます。

まずシミュレーションで確認してみると、やはり理論で証明されているように出力の速度はかなり変動してしまうことがわかります。

今度は、シミュレーションと同じ条件になるプログラムを作成して、速度指令値が台形パターンとなる電圧をモータに与えたときの速度を実測してみます。

シミュレーションと同じ結果で、ちょっとした負荷ですぐに速度は変動してしまうことが確認できます。

この無防備な状態で、例えばマイクロマウスなどの駆動に適用すると勾配やちょっとした段差などの障害でも望むような動作ができない場合が起こりえます。

そこで、より性能を向上すべくモータ速度をフィードバックした種々の方式で速度指令値（目標値）に対する追従性と外乱の影響を確認していきます。

PID制御のうち、比例と積分要素を補償器としたフィードバック速度制御を構成して性能を確認します。

ステップ応答時と同じ条件で速度指令値を台形パターンにしたものです。補償器の条件は比例ゲインを0.5, 積分ゲインを15にしたものです。

まずシミュレーションで確認してみると、この条件においては外乱の影響に関してはぼぼオープン制御と同じでほとんど改善効果は見られません。

シミュレーションと全く同じ条件で実測した結果をみても同様ですが、オープン制御と異なる点は速度指令値と実際の速度には定常偏差がなくなっている点です。

今回の条件ではゲインを比較的小さめに設定したこともあり、外乱の影響を抑制する効果はほとんどないといえます。もっとも、ゲインをより大きく設定すると外乱の影響を抑えることはできますが、ループ内全体のゲインをあまり大きくするとノイズ等の影響も受けやすくなりあまり好ましくはありません。

PI制御は定常状態の偏差をなくすには有効な方式ですが、外乱負荷の影響をなくすにはあまり効果が期待できないことがわかります。

今度は最もシンプルなフィードバック方式で、効果もあげやすいハイゲインフィードバック方式で検討してみます。

ここではこの方式を"ハイゲインフィードバック"と名づけてはいるのですが、実際にはそんなに大きなゲインは使用しないいたって実用的な方式です。

検証する条件は下記ブロック線図のとおりです。

上記の条件では入力から出力間の特性は実質10msの時定数をもった１次遅れ系となります。詳細はDCモータの簡単で実用的な速度フィードバック制御で解説しています。

まずシミュレーションで確認しますが、オープン速度制御やPI速度制御ではあれだけ負荷外乱の影響をうけていたのですが、この方式では同じ条件であるのにかかわらずほぼ打ち消されていて、速度指令値どおりに追従するサーボ運転となっています。

シミュレーションでは良い結果であったのですが、実測値ではどうでしょうか。

シミュレーションと同じ条件でプログラムを構成して実測してみました。制御アルゴリズムのプログラミングでは積分器を使用するPI制御よりも単純です。

ここでも結果はシミュレーションと同じく外乱の影響をキャンセルして安定して指令値に追従していることが確認できます。

ハイゲインフィードバック方式はシンプルな構造なのにも関わらず効果も抜群の方式でしたが、外乱抑制を高めるためにゲインを大きくすると同時に応答性も向上してしまうことが短所でした。

これで問題がなければよいのですが、外乱抑制と応答性を独立して設定したい場合に有効な方式が2自由度ロバスト制御方式です。

今回の場合もステップ応答で構成したものに速度指令値（目標値）を台形パターンにしたところだけが異なります。シミュレーションやプログラムを構成するのに容易な近似タイプを採用します。【実践で使えるDCモータのロバスト速度制御】参照

まずシミュレーションで確認しますが、ハイゲインフィードバック方式と比較すると、速度が指令値に対してわずかに遅れているのは、近似タイプのため、ロバスト補償器の時定数T_m2をモータ制御対象の時定数T_mと同じ50msにしているからです。

外乱抑制についてはハイゲインフィードバック方式と同じく十分な効果があることが確認できます。

制御アルゴリズムのプログラミングでは１次遅れのフィルターを構成する必要はありますが、そう複雑なものでもありません。

シミュレーションと同じ条件で実測したものでも外乱の影響がなく安定して指令値に追従していることが確認できます。近似タイプでも十分効果があることが確認できました。

一般的なフィードバック制御といえばPID制御がまず挙げられます。難しい理論抜きでも感覚的にゲインを設定して出力を調整することも多いです。

もともと安定しているEV3モータに多少の条件が変わっても一定の目標値を維持するための制御です。偏差eをゼロに近づけるためにPIDゲインのなかでP(比例)とI(積分)要素を使い、特に積分を重視します。

これまで制御システムを評価するシミュレーションはScilabなどのツールを使用してきました。設計評価のときには、制御対象の伝達関数もPI補償器も連続系のs関数でよいのですが、実機で実現する場合、PI補償器はプログラム内で離散系として記述する必要があります。

そのため、実機に近い最終的な動作確認にはExcelによるシミュレーションで評価するのが効果的です。モータなど実モデルは連続系の時間関数、例えばステップ応答時の１次遅れは1-ext(-t/T_m)で記述できます。補償器は離散系で実際のプログラムではサイクル周期を考慮して積分器等を記述します。

Excelにおいて１次遅れのモータ制御対象を連続系として、PI補償器を離散系とした場合の応答です。実際のプログラムではサイクル周期を5msとするために、シミュレーションで5ms刻みの離散系としています。

ステップ入力200時のシミュレーションです。入出力特性は積分ゲインを入れているため、２次遅れ系となっています。比例ゲインKpは0.5、積分ゲインを15とすると振動はおさまっていますが、積分ゲインが大きいため立ち上がりはややゆるやかです。比例ゲインがゼロの場合は積分ゲイン値によりすこしオーバーシュート気味になるかもしれません。

定常時には出力のモータ回転速度は制御器の積分要素のおかげで定常偏差はゼロに近づき安定していますが、立ち上がりの応答は任意に改善することができないことがPI制御の限界です。定値制御であればPI制御でも十分ですが、入力が変化する追従サーボ制御には適用は厳しいかもしれません。

Excelで動作に問題がないことが確認できれば、あとは同じ記述をプログラム内に移植するだけです。離散系のPI補償器の部分はすでにExcel内で記述しているので移植は簡単です。実機のプログラムでの注意点はシミュレーションでは現れない変数の初期値の処理などです。最終的なモータへの出力はPWMのデューティ比で設定します。

シミュレーションと同じ条件の実機の動作です。シミュレーションと同じ応答特性が実機でもみられ、出力速度はオープンPWM制御に比べて定常偏差がなくなり安定しています。

これまで、DCモータの速度をコントロールするのにPWMオープン制御方式、ハイゲインフィードバック制御方式、PI制御方式を行ってきましたがここで集大成のロバスト制御に挑戦したいと思います。

今回実機で実現する２自由度ロバスト制御の詳細はマイコンで実現するフィードバック制御のための基礎知識【発展編】で解説しています。

まずEV3Lモータに２自由度ロバスト制御を適用する場合のパラメータをシミュレーションツールScilabで評価しながら決定します。２自由度制御システムを実機で実現するには制御対象P(s)の規定モデルP_n(s)を逆システムとして離散化しなければならなく結構複雑になってしまいます。そこで、ここでは思い切って補償器で設定する時定数T_m2を規定モデルの時定数T_nと同じにして近似化します。

近似化すると２自由度ロバスト制御のブロック線図は以下のようにすっきりした形になります。

よくみるとハイゲインフィードバック方式のものに似ていて違いは入力に１次遅れフィルタG_ry(s)があることです。フィードバック部で外乱抑制を向上させてこの部分で応答特性を調整していると考えられます。

ここでもPI制御と時と同じようにまずExcelでシミュレーションして評価します。青色点線で囲まれた制御対象は連続系、緑色点線で囲まれたロバスト補償器は離散系で記述します。

外乱抑制に関してはフィードバックゲインCを大きくすればより効果があります。シミュレーションでは立ち上がりは設定した１次遅れ系時定数50ｍsで起動しています。

実機での２自由度ロバスト制御の応答です。時定数T_m2は50msが今回の近似システムの条件ですが、試しに、時定数T_m2を違う値に設定してみたところ、近似モデルにおいても時定数により立ち上がりに違いは多少現れていました。ここでは現れていませんが、外乱負荷を与えても出力の回転数はとても安定しています。

いわゆるロバスト制御の特長がよく現れていて比較的に簡単に実現できますので、ちょっとしたアプリケーションに積極的に採用できるのではないかと思います。

これまでは速度フィードバック制御について、定値制御のステップ応答で検証してきました。実践的な速度入力としては台形波パターンなどを与えることが多いので次回は入力に追従する速度サーボ制御について、各制御方式で比較検証していきたいと思います。

DCモータにギアを介したレゴ®マインドストームEV3Lモータの数式モデルは電気回路、機械の運動方程式から求められ、厳密には入力電圧から出力速度間の伝達関数は2次遅れ系となります。

本来の数式モデルは機械的伝達機構のモデル化も含むので結構複雑ですが、このモータのフィードバック制御を実践で取り扱うためにはできるだけ現実に即したモデルである必要があります。つまり、いくら高精度で高度に数式モデルを作成しても実際のパラメータ変動などで意味のないものになってしまうからです。

そこで、現物の特性を押さえたできるだけ簡素化したモデル化をするのが現実的です。

モータ端子電圧を入力、EV3Lモータの回転速度を出力としたときの伝達関数は1次遅れ系で近似できますので、入力の端子電圧をステップ状に与えたときの回転速度の変化から時定数T_mを調べます。最大電圧時（7.2V）のときの速度は410p/sであるため、伝達関数ゲインK_mもわかります。

これで伝達関数の近似モデルが求まりました。実際の波形をみても、1次遅れですので近似モデルで設計するのが実践的であることがわかります。

EV3Lモータの入力に端子電圧7.2[V]を与えるとギアを介した負荷側の回転速度は時定数T_mで起動して、定常で410[p/s]になるということです。実際のプログラムにおいては7.2Vの指令をあたえるのにPWMのデューティ比（この場合は100）を指定することになります。

エンコーダからのパルスを単なるモニターでなく速度フィードバック用のセンサとして使用する場合はセンサによる速度の細かな変化データが必要なため、本来は分解能の高い高性能のエンコーダを使用する必要があります。

今回のEV3Lモータのようにエンコーダが負荷側に装着していて分解能が比較的粗い（180p/r）場合は、測定精度を上げるために一工夫必要です。測定には簡易的なエンコーダでも高精度エンコーダに劣らない工夫を施しています。

速度フィードバックの中でも最も簡単で効果の見られるハイゲインフィードバック方式で特性改善を試みます。ハイゲインフィードバック方式はフィードバックゲインC₁と出力調整用ゲインC₂で構成します。

ハイゲインフィードバック方式のブロック線図です。 フィードバックゲインC₁を上げるほど特性が改善されるのでハイゲインフィードバックと呼ばれます。ただし、現実的にはノイズなども増幅してしまうことになるため、実現できる範囲は限られます。

マイコンで実現するフィードバック制御のための基礎知識【解析編】の内部安定性で解説していますが、フィードバックゲインC₁が比較的小さい場合はノイズから出力までの時定数が大きくローパスフィルタ特性が効果的なのですが、大きくなるに連れて時定数は小さくなり（カットオフ周波数は高くなる）、ノイズの影響は受けやすい傾向になります。

今回のEV3Lモータをハイゲインフィードバック方式に適用するとこのようになります。オープンループ制御との大きな違いは速度目標値がシステムの入力となっていてモータ回転速度をフィードバックさせている点です。

青点線で囲まれた部分はEV3Lモータそのものの制御対象で、緑点線部がハイゲインフィードバックを施したコントローラ部です。ここの部分はプログラミングで実現します。

実機に制御理論を適用する場合に大切なポイントは扱う信号の次元（単位）を常に意識することです。ここではあえて次元を揃えるために変換係数K_nを設けていますが、これはあってもなくても結果は同じものになります。

プログラムの記述をできるだけ簡素化するためにさらにモデルを近似化します。実際のプログラムにおいては電圧指令にアナログ電圧値そのものを指定するのではなくPWMのデューティ比（0-100）を指定します。つまり最大入力時のデューティ比100%に対する最大速度パルスを400p/sに近似すると以下のようにとてもシンプルなブロック図となります。

ここで改善後の最終的な入出力間の特性を決めるためのゲインC₁とC₂を決定します。もともとの制御対象の1次遅れ特性T_m=50ms をフィードバックを施すことでT_m2=10msへ改善するものとします。

この場合、ゲインC₁は一意的に決定されてC₁=4となります。その結果、ゲインC₂=5となりますが、これは必要に応じて最終的に出力をみながら微調整します。

これで入力を速度目標値にしたときの出力速度までの伝達関数G_ry(s)の特性は時定数T_m2の1次遅れとなります。時定数も外乱の影響も1/5倍となっています。C₁のゲインにより特性改善はできますが、実現できる範囲で設定します。応答と外乱抑制は独立して設定できないところがこの方式の限界です。

詳細はマイコンで実現するフィードバック制御のための基礎知識【発展編】のハイゲインフィードバック方式との比較を参照してください。

EV3 Lモータが時定数50msの1次遅れ近似モデルであることが算定できましたのでいよいよこのモデルをベースとしたさまざまな制御システムで動作を確認していくことにします。

実機での制御システムを実現するとシミュレーションでは現れなかった現象がでるものです。これはドライバからモータへの入力制限であったり、センサや処理内容の分解能などにより想定外の結果になる場合もあるので、可能な限り条件は織り込んでおく必要はあります。

EV3 LモータはDCモータベースモデルのため、オープン制御でもPWMパルス出力のデューティ比を変えるだけで速度を調整できます。

下図ではPWM比を48[%]に設定したときのエンコーダからのパルス[p/s]を示しています。ドライバに与えた電圧V_m(7.2V)の約半分であることで速度も半分となっています。

95%PWMduty比のステップ入力を与えた場合のオープンループのPWM制御の応答です。縦軸はエンコーダから得られる速度信号で単位は[p/s]です。横軸は経過時間で単位は[ms]です。

起動後、時定数T_m (50ms)で目標値に到達しています。

次に、速度目標入力を200[p/s]に設定したハイゲインフィードバック方式のステップ応答を示します。 フィードバックゲインC₁を１から4に設定した場合です。応答波形をみるとオープンループPWMの場合に比べて、ハイゲインフィードバック方式はゲインC₁を大きくするにつれ時定数が小さくなり応答が改善されています。

ハイゲインC₁を3より大きくしても応答が変わらないのはモータ電圧出力制限によるからです。そういう意味で、最大入力400[p/s]のステップ信号を与えた場合は100%で起動しますのでゲイン C₁ を変えても応答の改善はありません。

これまで、速度フィードバック制御として最も簡易的で効果の高いハイゲインフィードバック方式でオープンループPWM制御方式に対して改善できるかを検討してきました。 EV3Lモータはエンコーダを持っているのですが、分解能はあまり高くなく入力は電圧制御型ドライバPWMデューティ比を変えているだけなので高度なフィードバック制御を施すには制限はあります。

次回はこのような条件下で速度PI制御や２自由度ロバスト制御を適用するとどんな結果になるか検証してみたいと思います。

マイコンで実現するフィードバック制御のための基礎知識【応用編】でDCモータの開ループ特性について解説しました。ここではマイコンのPWM出力を利用した電圧制御による速度制御について実機を使って確認します。

DCモータは無負荷という条件付きではありますが端子電圧を調整することで回転速度もそれに比例して調整できます。そこで、端子電圧にPWMで設定したパルス状電圧を与えて回転させてみることにします。

実機にはレゴ®マインドストームEV3のLモータを使用します。このモータには多段のギアが組み込まれていてギア比は48:1、エンコーダは負荷側に装着されていて互いに1/4周期ずれたA相、B相のパルスを出力します。各相パルスの実測値分解能は180P/Rです。つまり1回転あたり180パルスが出力されます。負荷側の定格回転数は160-170r/minとなっています。

このモータモジュールを使うにあたって、モータ単体のものでなくギアも含めた一体のものとして特性を調べることにします。つまり、入力はモータの端子電圧で出力は負荷側の回転速度となります。

モータは誘導性負荷の典型的なもので、電力も大きく、マイコン出力を直接つないで運転させることはできません。そこで、マイコン用の電源とは別途のモータドライブ用電源V_Mを用意し、トランジスタなどの増幅素子を介してマイコンからの運転信号でドライブします。

一から始めてトランジスタ等を組み合わせてドライブ回路を構成することもありますが、ちょっとした用途ではモータを運転するための専用のICを使用することをおすすめします。必要な機能が内蔵されていてサイズは小さい割にドライブの容量が大きく価格も低いためです。

これはモータドライバと呼ばれる一種のパワーアンプです。 詳細は割愛しますが、ドライバにはトランジスタ、FET等のパワーエレクトロニクス回路が内蔵されていてマイコンからの信号によりモータへの任意の電圧や電流を調整できるものです。ドライバの種類により電圧制御タイプや電流制御タイプがあります。 今回使用するTB6612FNGは電圧制御タイプです。

モータドライバTB6612FNGはドライバに接続したモータ用電源V_Mをマイコンから与えたPWMパルスのデューティ比率に応じてモータ端子電圧とするICです。モータの回転方向はIN1とIN2を組み合わせて切り替えます。

DCモータはブラシを内蔵しているために整流子との間でノイズが発生し、マイコンに影響を及ぼします。 そこで、通常はモータ端子にセラミックコンデンサ（100nF）をノイズ対策用としてつないでおきます。小型モータであればこれでよいと思います。今回使用するTB6612FNGのドライバモジュールはコンデンサを内蔵しています。

マイコンからのPWM出力によりモータ端子電圧はパルス状になりON期間に駆動トルクによるドライブ運転、OFF期間に逆起電力によるブレーキがかかる運転となります。端子電圧がパルス状でもモータにはインダクタンス成分がありますので電流は電圧の1次遅れとなり連続になります。

電流の変化率はモータのインダクタンスおよび抵抗値成分で決まる電気的時定数L/Rによりますが、PWMのスイッチング周波数が低いと電圧オフ期間（ブレーキ運転時）に電流は低下しすぎてしまい、モータ軸慣性があるとはいえ発生トルクに脈動が生じることになり、逆に周波数が高いと電流はなめらかになりますがノイズやスイッチング損失が大きくなるのでやみくもに高くもできません。

一般にモータ電気的時定数に対して10倍程度のスイッチング周波数にすればよいといわれています。 とはいえ、電気的時定数が不明であるため、まずは10kHzあたりのPWMスイッチング周波数で試してみようとおもいます。

下表はドライバ仕様書に記載の運転モードについてです。入力IN1とIN2の組み合わせでモータ回転方向を指定したり、俗に言うRUN信号の機能として使用します。PWMは信号のパターンがモータ端子電圧になるものです。ON時に駆動して、OFF時にモータからの逆起電力が発生してブレーキがかかることになります。PWM入力はマイコンのPWMタイマ出力に接続します。

電動モータのドライブ機器でインバータはよく耳にすることがあると思います。これもパワーエレクトロニクス搭載のモータドライブ機器ですが、インバータは3相誘導モータ用で交流出力の周波数を変更することでモータ速度を任意に変更する機器です。

DCモータのドライブ回路はこのインバータ出力3相のうち単相部分を使ったものに相当し、4象限チョッパ回路とも呼ばれるものです。 モータ回転の正転逆転、発生トルクの正と負を4通り組み合わせた運転ができるのことから4象限回路といいます。ドライブ運転とは回転方向にトルクを発生して駆動させる通常運転のことで、ブレーキ運転は回転方向とは逆のトルク、つまり逆起電力を発生させながらブレーキをかける運転のことです。

モータドライバTB6612FNG ではドライバへの入力IN1とIN2の組み合わせで第1象限から第4象限の運転を切り替えます。

チョッパ回路は下図の4つのトランジスタなどのパワースイッチング素子(Tr1,Tr2,Tr3,Tr4)で構成されています。PWMのパターンに応じてこれらのスイッチング素子のON・OFFで組み合わせますが、スイッチ切り替えの過渡期に組み合わせが重なって同時にONとなり短絡になってしまうのを防止するためにデッドタイムというものを設けています。

ドライバTB6612FNGはブレーキ運転時にコイルで発生した電力を回路内で吸収する回生方式ですが、電流制御タイプTB67H450FNGでは電源に返す回生をおこなっています。ドライバによりスイッチング素子の組み合わせは違うようです。

VR(可変抵抗)からのアナログ入力信号を速度指令信号として与えます。モータドライバへのPWMパルスデューティ比率はアナログ入力値と連動させて速度を変化させるプログラムです。

今回はDCモータのオープンループ制御ですので速度指令値に相当するPWMデューティ比率を変化させてモータ回転速度が変化する様子を確認してみます。

エンコーダ、シリアル通信はモニタ用です。

出力側にはドライバへモータ電圧指令としてのPWMタイマ出力を与えます。ドライバのIN1/IN2には運転モードにあったロジックの組み合わせをプログラムで指定します。

USER SWを押すたびに正転(CW)、ブレーキ停止、逆転(CCW)、 ブレーキ停止のサイクルを繰り返すプログラムとしています。

ドライバ入力は内部でプルダウンとなっていて6Vまでのロジック電圧入力仕様なのでマイコンはプッシュプル出力とします。

EV3Lモータのコネクタ仕様です。エンコーダからは電圧パルスが出力されます。

マイコンボードNucleo-F103RBでEV3Lモータを動かす全容です。 レゴ®モータのソケット部は特殊なので汎用の電話用モジュラーコードに変換しました。

PWM出力のduty比とエンコーダによるモータ速度をシリアル通信モニターにてPCに表示します。モータ電源V_Mが7.2Vの場合、ボリュームを調整してPWMデューティ比を0から100まで変化させると 速度は比率にほぼ比例して0から約40まで変化します。

速度40とは100msサイクル内で計測したエンコーダパルス数なので1秒当たり400パルス、つまり400/180回転に相当します。1分当たりだと約133回転です。規定の回転数(最大）が160-170rpmだそうですので モータ電源V_M を最大の9Vにするとほぼ一致します。

無負荷運転時の入力指令値に対する速度です。端子電圧が極めて小さい領域(duty比0-5％)ではモータ軸、ギアの摩擦などの影響で起動しませんが、一度回転始めると出力速度はデューティ比つまり端子電圧にほぼ比例しています。

PWMスイッチング周波数を5kHz, 20kHzおよび40kHzに変更してドライブさせてみたのですが、気になるトルクの脈動はなく状態の違いはみられませんでしたので、10kHzのままでいいかと思います。機会があれば電流波形で確認してみたいと思います。

DCモータのオープンループ制御なので、無負荷であれば回転速度は端子電圧に比例しているのですが少しでも負荷をかけると速度は簡単に変動します。負荷によらず安定した速度を保つためにはセンサーによる速度フィードバック制御が必要です。次回はフィードバックループで構成した速度制御システムを実機にて検証したいと思います。

アドバンスト制御のなかでも、本編ではロバスト制御を解説していきます。

ロバスト制御というものはDCモータなどの物理的な制御モデルを数式化した際のモデル化誤差や負荷変動などの制御対象P(s)の入力側に加わる外乱、およびセンサノイズなど制御対象の出力側に加わる外乱に対して制御出力が影響を受けにくい制御システムのことをいいます。

ロバスト制御では、端的にいうと外乱d、観測ノイズn、および多少のモデル誤差化⊿があっても入力rから出力ｙまでの伝達関数 G_ry(s) が設定した特性（例えば時定数T_rの１次遅れ）になるように補償器C(s)を設計します。

ロバスト制御のうちでも、PID制御なみに簡易に実現できるものであれば、ちょっとしたアプリケーションにも積極的に適用したいものです。本編ではそんな比較的気楽に扱えるような実用的なものの検証をしていきたいと思います。

従来のPID制御はフィードバックループ内にあるPID補償器の比例・積分・微分ゲインをそれぞれ設定して出力を調整するものです。各ゲインの決め方は経験に基づいた値を試行錯誤的に決めることも多いようです。

PID制御の利点は理論的なものを理解していなくても手軽に感覚的に設定できることですが、ゲインにより調整できるのは応答性であって、外乱などに対しては特性の根本的な改善はできません。

また、PID制御を始めとする従来のフィードバック制御ではループ内のゲインを上げる（増幅）ことによって、安定性や応答性を改善することはできますが、同時にモデル化誤差やノイズなど望まないものも増幅してしまうことにより想定したとおりの性能がだせない場合も起こりえます。単なるフィードバック制御では精度や応答性の向上を望むには限界があるといえます。

そこで、ロバスト制御を実現するのにあたって応答特性と外乱抑制を独立して設定できる２自由度制御システムと呼ばれる制御方法があります。ロバスト制御には他に外乱やモデル化誤差を推定して変化分をキャンセルする外乱オブザーバー的な方法もありますが、本編では２自由度制御を取り扱います。

2自由度制御にもいろいろありますが、今回取り扱うものは、最もシンプルで誰でも検証しやすく、プログラミングなどでも実現しやすいものです。

入力rから出力ｙまでのブロック線図は下図の形になります。通常のフィードバック制御用ループに加えて入力から分岐した情報を加えた形になっています。

上のブロック線図の配置を並べ替えると下図のような等価ブロック線図となります。これが意味しているところはフィードバック部とフィードフォワード部で構成されていて、外乱抑制はフィードバック補償器C(s)、応答特性はフィードフォワード部のG_ry(s)およびP_n(s)で改善します。これらが干渉することなく独立して設定できることから２自由度と呼ばれています。

マイコンで実現するフィードバック制御のための基礎知識【応用編 】ではDCモータの特性についてモータ回転速度は入力電圧を変化させれば調整できることを説明しました。無負荷であれば回転速度は入力電圧にほぼ比例していますので入力をu、出力をｙとすると下図のような１次遅れで近似モデル化できます。

近似モデルゲインK_m、時定数T_mは実際の入力を与えたときの回転速度yを実測して得られるものに相当します。これは一種のパラメータ同定と呼ばれますが、DCモータの実際の慣性モーメントJや粘性摩擦Dといった物理的なパラメータでなく近似モデルのパラメータです。

入力uから出力ｙまでの物理的パラメータが K_mとT_mに集約されていてより実用的なモデルです。 入力は電圧でも電流でもよく、パラメータ K_mとT_m はそれに応じた値になります。 K_m は伝達関数のゲインなのですが、入力から出力までの変換係数といった捉え方がわかりやすいのではないでしょうか。

DCモータを制御対象にした2自由度制御システムによる速度制御を解説します。

制御対象P(s)には入力uを電圧または電流、出力yをモータ回転速度とした1次遅れモデルとします。この実モデルは変動の可能性があるパラメータK_m, T_m を持ちますが、この規範モデルをP_n(s)とします。

規範モデルP_n(s) のパラメータは実モデルP(s)のパラメータ同定により得られたパラメータK_m,T_mをそれぞれ規範モデルパラメータ K_n,T_n とし、フィードフォワード補償器内で使用するものです。

フィードフォワード補償部は特性改善後の入出力間目標応答特性G_ry(s)および規範モデルの逆システムPn (s) ^-1 で構成され、フィードバック補償部には、ロバスト特性を決定するゲインC（制御対象により今回は定数）が入ります。

特性改善後の入出力間目標応答特性G_ry(s)は時定数T_m2の1次遅れとしますが、時定数T_m2は実現できるレベルで設定する必要はあります。

制御対象の実モデルP(s)と規範モデルP_n(s)に誤差変動がなく P(s) ＝ P_n(s) である場合、入力ｒから出力ｙまでの伝達関数は設定した目標応答特性G_ry(s) となります。

制御対象の実モデルP(s)と規範モデルP_n(s)に誤差変動がある場合、下図の⊿(s)が変動分です。フィードバックゲインCが大きいほど変動誤差の影響は小さくなるので 設定した目標応答特性G_ry(s) に近づきます。

今回設定したフィードフォワード補償のタイプでは変動があっても、実モデルパラメータと規範モデルパラメータが互いに相殺しあうかたちになっているのも特徴です。

ここが、2自由度ロバスト制御の本質的な部分です。外乱特性はフィードバックゲインCにのみ関連していて応答特性を決定する 目標応答特性G_ry(s) とは無関係です。
ゲインCが大きくなるほど出力への影響は小さくなることがわかります。この値も実現可能な範囲で設定します。

今回の2自由度制御では 目標応答特性G_ry(s) を1次遅れとしているため、負荷トルクが定常負荷のように一定で大きい場合は、定常誤差が発生してしまいます。この誤差を小さくするためにはフィードバックゲインCを大きくすると零に近づきますが収束するわけではありません。

どうしても、出力を目標値に一致させるためには外側に定常偏差をなくすためのPIフィードバック補償ループを追加して適当な比例、積分ゲインを調整します。

実装のポイントはロバスト制御部をできるだけ高速で処理し、PIフィードバックによるサーボ補償のループをそれよりも遅い処理にして相互干渉による影響をなくすことです。

検証した結果の時間応答をシミュレーション（ Scilabを使用 ）して確認します。

DCモータに開ループ状態でステップ入力時にパルス状の負荷外乱を与えたときの応答です。ちょっとしたパルス状の負荷でも出力に大きな影響を与えることがわかります。

今度はステップ状の負荷外乱を与えたときの応答です。 指令値から大きく下がったところで負荷に応じて発生トルクと釣り合ってしまっています。

目標応答特性G_ry(s) の時定数T_m2を50ms、ロバスト補償器ゲインCを0.5とした条件でパルス状外乱を与えたときの出力応答です。制御対象には、規範モデルに対してKmは+30%、Tmは-20％の モデル誤差に加えて、時定数10msの1次遅れ寄生要素を追加しているので2次遅れ系となっていますが、多少のモデル化誤差では出力は 目標応答特性G_ry(s)の特性を維持したまま、外乱を短期間で抑制しているのがわかります。

ロバスト補償器ゲインCを更に大きくすると外乱やモデル化誤差の変動の抑制効果は向上します。

ロバスト補償器ゲインC だけを3に増加してから外乱をステップ負荷としたときの応答です。ここでも 出力は 目標応答特性G_ry(s)の特性を維持したまま、外乱の影響が出力にほぼ現れず、ロバスト制御の効果が見られます。応答特性と外乱抑制特性を独立して設定できる2自由度制御の効果をよく表しています。

簡易的な速度制御特性を改善する方式にハイゲインフィードバック方式があります。出力側をゲインC₁を介してフィードバックし、 ゲインC₁とC₂の値を組み合わせて外乱を抑制しながら入出力間のゲインを1に近づけことができるとても簡単な方式です。

外乱特性を向上するためには 外乱抑制用フィードバックゲインC₁を上げれば改善できます。C₁の大きさに頼るところからハイゲインフィードバックと呼ばれます。

応答特性は全体のゲインが1になるようにC₂により調整することで求められますが、外乱抑制用ゲインC₁の大きさに依存するうえに、任意の応答特性を得ることはできません。また、外乱を抑えるためのハイゲインに頼ることになるため、出力側センサーからのノイズの影響を受けないように注意する必要があります。

パルス状の 外乱負荷として加えたときの応答をシミュレーション結果です。外乱パルスはフィードバックゲインC_１により抑制されていますが同時に応答も影響をうけてゲインやモデル化パラメータによっては過応答になってしまいます。

外乱抑制と応答特性はフィードバックゲインC_１により決定づけられてしまいますが、実現できる範囲で設定できるようであればハイゲインフィードバック方式は最も簡単で実用的です。

速度制御モデルが外乱やモデル化誤差の影響を受けない目標応答特性G_ry(s)でモデル化されていると簡単に位置決め追従制御に発展できます。ここでは、産業用途ではよく目にする速度制御用フィードバックループの外側に位置制御用フィードバックループで構成されているタイプとは異なる、加速度指令方式位置決め追従制御を解説します。

速度応答が外乱やモデル化誤差の影響をうけない目標応答特性G_ry(s) である場合、速度の微分である加速度を指令とした場合、入力から出力までの伝達関数は目標応答特性G_ry(s)を積分したものになります。

応答 G_ry(s)の時定数T_m2は実現できる範囲で十分小さく設定することが好ましいです。

加速度指令値を生成するために予め位置θ₀、速度θ^'₀、加速度 θ^"₀ の追従軌道の目標値を作成しておきます。加速度参照値θ^"₀をフィードフォワード項として、速度θ^'₀、位置θ₀参照値と実際値 θ, θ^' との誤差にそれぞれゲインK_v, K_pをかけたものをフィードバック項として加速度指令値 θ^"refとします。

ゲインK_vとK_pは 2次遅れ系の応答を参考にして目標の速応性および減衰性を考慮して簡単に決定できます。設定した2次遅れ系の応答で起動時の誤差が収束すると⊿θ(=θ₀-θ)は0、つまり実際位置θは遅れなく参照値θ₀に追従することになります。

上式が成立するのは速度系にロバスト制御が施されていて速度指令値θ^'ref ≒ 速度θ^' となることにより加速度指令値θ^"ref ≒ 加速度θ^" とみなせるからです。

上式だけ見ているとゲインK_V、K_Pは任意に決めても問題なさそうですが ゲイン選定を適当にすると応答は乱れる可能性があります。 これらのゲインで2次遅れ系規範モデルを構成することになるからです。

位置決めサーボ系として見た場合、指令値に目標位置θoのみ与えた場合は目標値θoから出力θまでの伝達関数は2次遅れ系の規範モデルK_P/(s²+K_Vs+K_P)となり、出力θは遅れて追従します。

規範モデルに対して指令値に加速度θ^"oおよび速度θ^'oを含めることにより、それらが位置決めサーボ系ではフィードフォワード的な役割を果たし、出力θは定常偏差がなくなり目標値θoに遅れなしに追従するようになるのです。見方を変えると規範モデルの逆システムを構成するのと等価になります。

つまり、システムとしては規範モデルが安定であることが必要で、仮に不安定な極をもつ規範モデルに対して、不安定な零点をもつ規範モデルの逆システムを構成すると、数式上では相殺されるので問題がなさそうですが、実際は必ず遅れ要素やモデル化誤差および外乱等が存在するため、相殺されることはなく不安定なままであるということです。

速度系目標応答特性G_ry(s) の時定数T_m2を10ms、ゲインK_vとK_pは 速応性ω_n=10,減衰 0.8としてK_v＝16, K_p=100 としたときの追従性をシミュレーションしてみました。比較的緩やかな目標値の場合ですので時定数が大きめでも位置は遅れなく追従できていることが確認できます。