ロボットをスムーズに走らせるシステム制御

設計／手法／テスト

技術解説

連載 :

文系女子の「ETロボコンで組み込みデビュー」

2011年6月27日(月)

吉田彩花舟元拓斗

P制御の実装

[吉田]分かったような、分からないような。
[舟元]自動車の運転に例えると、前の車と適度な一定の車間を取りながら運転したい場合、車間距離が開いたらそれに合わせてアクセルの踏み込む量を調整するよね？車間が適度な場合はアクセルの状態はそのままにしておくでしょ、これがP制御なんだ。ON/OFF制御の場合は使える車速が100Km/hと0Km/hだけで、車間が開いたら一気に時速100Km/hにして、車間が縮まったら時速0Km/hにするみたいなモンだね。今回の場合は車間が目標光量で、アクセルの踏み込む量が、走行体の旋回量になるんだ。
[吉田]なるほど、100Km/hと0Km/hでしか車間を調整できないのなら一定に保つのがむずかしい、それは今回のライントレースでも同じってことだね。私たちずいぶん乱暴な動作をしていたんだね。あ、乱暴な制御ね。
[舟元]そうそう、じゃあ、偏差0の時はまっすぐ走行させることにして、偏差が生じたときには、（偏差×比例ゲイン）で旋回をさせるようにしてみよう。
[吉田]分かった。ソースコードを修正してみるね。

#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
#include "balancer.h"

#define PORT_MOTOR_L NXT_PORT_C
#define PORT_MOTOR_R NXT_PORT_B

#define PORT_GYRO NXT_PORT_S1
#define PORT_LIGHT NXT_PORT_S3
#define PORT_TOUCH NXT_PORT_S4

#define GYRO_OFFSET (623)           /* ジャイロセンサーのオフセット */
#define WHT (540)                   /* 白の光量 */
#define GRY (630)                   /* マーカーの光量 */
#define BLK (755)                   /* 黒の光量 */
#define KP (0.38)                   /* Pゲイン この値を色々変えてみる */

void user_1ms_isr_type2() {
}

TASK(Main) {
    S8 pwm_l, pwm_r;
    F32 forward = 50.0, turn = 0.0;
    U8 old = 0;

    ecrobot_set_light_sensor_active(PORT_LIGHT);

    /* SW処理 */
    while (1) {
        U8 now = (ecrobot_get_touch_sensor(PORT_TOUCH) == 1) ? 1 : 0;
        if ((1 == old) && (0 == now)) {
            old = 0;
            break;
        } else {
            old = now;
        }
        systick_wait_ms(100);
    }

    U16 target = (WHT + BLK) / 2;   /* 目標光量は黒と白の平均値とする */

    nxt_motor_set_count(PORT_MOTOR_L, 0);
    nxt_motor_set_count(PORT_MOTOR_R, 0);
    balance_init();

    while (1) {

        /* 現在の光量を取得する */
        U16 light = ecrobot_get_light_sensor(PORT_LIGHT);

        /* 操作量 = 偏差×比例ゲイン　*/
        turn = (light - target) * KP;

        /* 上下限値チェック */
        if (100 < turn) {
            turn = 100.0;
        } else if (turn < -100) {
            turn = -100.0;
        }

        balance_control(
            forward, turn,
            ecrobot_get_gyro_sensor(PORT_GYRO),
            GYRO_OFFSET,
            nxt_motor_get_count(PORT_MOTOR_L),
            nxt_motor_get_count(PORT_MOTOR_R),
            ecrobot_get_battery_voltage(),
            &pamp;wm_l, &pwm_r);
        nxt_motor_set_speed(PORT_MOTOR_L, pwm_l,1);
        nxt_motor_set_speed(PORT_MOTOR_R, pwm_r,1);

        systick_wait_ms(4);
    }

}

[吉田]よし。できた。比例ゲインは KP として定義したから、これを調整しながら動かしてみるね。

比例ゲインを調整するには、ソースコード上の

#define KP (0.38)                   /* Pゲイン この値を色々変えてみる */

を、修正します。

[舟元]うん。比例ゲインは、何通りか試してみて一番良さそうなものを使おうか。

PID制御(KP=0.12)を動画で見る

PID制御(KP=0.38)を動画で見る

PID制御(KP=1.4)を動画で見る

[吉田]ON/OFF制御とP制御だと動きが全然違うね。P制御の場合はロボ太もキレイにライントレースしてる！でも、P制御でも比例ゲインの値によってロボ太の動きが違う。まとめるとこんな感じかな。

制御方法	結果
ON/OFF制御	とにかく左右にジグザグ
P制御ゲイン 0.12	ゆったりと左右に揺れてる感じ
P制御ゲイン 0.38	キレイにライントレースしてる
P制御ゲイン 1.40	せわしなく左右に揺れてる感じ

[舟元]うん。さすが文系。表現に奥行きがある。でね、「適切な比例ゲインは設計者が決めないといけない」というのはこのことなんだよ。ゲインが小さい（0.12）場合は、操作量が小さいから時間を掛けて目標光量に近づこうとしてるので低周期で左右のジグザグ動作（目標光量を超える・戻る）を繰り返している。逆に、ゲインが大きい（1.4）場合は、操作量が大きいから短い時間で目標光量に近づこうとして高周期で左右にジグザグしてる。でもゲインが適切（0.38）な場合は、左右のジグザグ動作が無くなって綺麗にラインをトレースするんだ。

比べてみる

[吉田]ゲインって大事なんだね。でも、やってる計算は偏差にゲインを掛けてるだけ。凄くシンプルだよ。
[舟元]そうなんだ。制御則を知ってると簡単な計算をするだけでクララの動きをこんなにスムーズに出来るんだ。今回はゲインは適当に決めたけど、決め方は色々あるんだよ。それに省略したI制御、D制御も押さえないとね。
[吉田]（クララ？ロボ太でしょ！）制御面白いね！…この辺はIT系のソフトウエアでは出てこないところだから萌えるわぁ！頑張らないと！
[舟元]そうだね！その辺も突っ込んで行きたいところだけど、ETロボコンの技術教育1の案内が来てたよ。制御の話はいったんこの辺にして技術教育1に行く準備をしよう。
[吉田]教育かぁ…自己研鑽の日々だよ。ところで技術教育1って何するの？
[舟元]ETロボコンのサイト（ETロボコン2011大会詳細 | 技術教育【ETロボコン2011公式サイト】）に内容について書いてあるよ。「モデリング入門」だって。
[吉田]モデリング入門？！モデリングとか初めて聞いたんですけど…、でも、何か伸び代を感じる。参加すると私のエンジニア力が伸びる気がするよ！
[舟元]じゃあ、僕たちのチームがエントリーしている北陸地区の技術教育1に行ってみよう。