C言語でEV3開発(15)
TOPPERS/HRP2 EV3でPID制御

2021年1月31日

どもです。
今回のエントリーは、コレまでのエントリーから内容を変えて、モーターの動作の制御をします。
制御方法は、「PID制御」です。
ちなみに、今回のエントリーからは使用するOS/環境を

「TOPPERS/EV3RT」

に変更します。

1.環境のセットアップ

省略します。
なぜなら、ここに詳細な手順が記載されています。
環境がWindowsでもMacでもOK!
上記のサイトを確認しながら、がんばってセットアップして下さい。

なお、当方の環境は、以下です。

OS Windows7 Professional
ServicePack 1(64bit)
CPU Core i7-3770K
メモリ 16.0GB
Ev3のOS TOPPERS/EV3RT
β6-2

2.制御方法 – PID制御

今回は、「PID制御」によるモーターの制御を行います。
PID制御は、使用されることが非常に多い自動制御方法です。
この方法は、比例/積分/微分の組み合わせで制御を行う方法です。
比例/積分/微分のそれぞれに対して係数を設定・調整することで、細かな制御・スムーズな制御が可能になります。
なお、以下のサイトに係数の設定について記載されています。
http://www.picfun.com/motor05.html
まずは、このサイトに記載された方法で係数を算出、シミュレーションを行います。

3.シミュレーション

シミュレーションについては、多くのシミュレーションソフトがあります。
しかし、今回は手元にあり、かつ一番手っ取り早かった「Excel」を用いて、シミュレーションを行ってみます。
なお、MindStormではモーター入力の単位は「%」です。
なので、入力値は0~100の間で設定することが可能です。

さて。
シミュレーションしてみた結果は、以下の通りです。
このとき、目標値:40%/Kp=0.10/Ki=0.05/Kd=0.10 としています。
(※コレは、上記サイトの計算式に従って係数を算出、その後調整を行った後の結果です。)

シミュレーション結果

グラフを見ると、出力地が徐々に目標値に近づき、目標値付近になると一定の値が継続していることが分かります。
従って、上記の係数を設定することで、目的のPID制御が実現できることがわかります。

4.実装~実機動作

いきなりですが、実装は以下の通りです。

/*****************************************************************************/
/*                                  変数定義                                 */
/*****************************************************************************/
int left_motor_config;
int right_motor_config;
int left_motor_power_current;
int right_motor_power_current;

/*****************************************************************************/
/*                                外部変数宣言                               */
/*****************************************************************************/
extern int left_motor_power;
extern int right_motor_power;

/*****************************************************************************/
/*                                  定数定義                                 */
/*****************************************************************************/
const motor_port_t left_motor_port = EV3_PORT_A;
const motor_port_t right_motor_port = EV3_PORT_D;

/*****************************************************************************/
/*                                  関数実装                                 */
/*****************************************************************************/
/**
    *  算出されたモーター出力で、モーターを動作させる。
    */
void motor_set_power(void) {
    ev3_motor_set_power(left_motor_port, left_motor_power);
    ev3_motor_set_power(right_motor_port, right_motor_power);
}

/**
    *  モーターの動作出力を取得する。
    */
void motor_get_power(void) {
    left_motor_power_current = ev3_motor_get_power(left_motor_port);
    right_motor_power_current = ev3_motor_get_power(right_motor_port);
}
/*****************************************************************************/
/*                                  定数定義                                 */
/*****************************************************************************/
static const int Kp = 10;
static const int Ki = 5;
static const int Kd = 10;

/*****************************************************************************/
/*                                  静的変数                                 */
/*****************************************************************************/
int integral;
int diff;
int diff_prev;

void calc_motor_power(void) {
    int ddt;    //微分要素値
    int power = 0;

    //PID制御
    diff = target_motor_output - left_motor_power_current;
    ddt = diff_prev - diff;
    integral += diff;
    
    power = (Kp * diff + Ki * integral + Kd * ddt) / 100;
    
    left_motor_power = power;
    right_motor_power = power;
    
    diff_prev = diff;
}

処理周期は10msecに設定、同じ間隔でモーター出力値とモーター入力値(実際の動作値)を測定しました。
なお、実測の際は空転状態で行っています。

5.実行結果

そして、その結果が以下。
右モーターと左モーターのモーター出力値と入力値、及びシミュレーション結果をそれぞれグラフにしています。

左モーター比較シミュレーション/指令値/動作値比較(左)
右モーター比較シミュレーション/指令値/動作値比較(右)

グラフを確認すると、シミュレーション結果に対して実測値は振動しているものの、目標値に近づいています。
従って、目標に合致した制御ができています。

6.まとめ

以上、今回はMindStormのモーターのPID制御を行いました。
チューニングした結果、目的の制御ができていることが確認できました。

ではっ!