Help us understand the problem. What is going on with this article?

ETロボコンのEV3カラーセンサが持つ気づきにくい罠について

こんにちは.kawanon868と申します.
DSPシステム部というチーム名で,ETロボコンのデベロッパー部門アドバンストクラスに参加しています.

戦績として,2018年地区大会では競技成績 4/6位(L, Rコースともにゴールゲート通過後のライントレースでコースアウト)と惨敗を喫しましたが,2019年地区大会では念願の競技成績優勝を果たしました(ともにアドバンストクラス).

2018年大会終了後にコースアウトの原因を調査したところ,EV3 カラーセンサがもつ仕様書に記載のない隠れた特性(罠)が原因でした.今回はその罠について検証したいと思います.

EV3 カラーセンサについて

EV3 カラーセンサはLEGO Education社のカラーセンサで,同社製品であるMindstorms EV3に接続して使用できます.
主な仕様はETロボコンおなじみのAfrel様がまとめている通りです.
サンプリングレートが1,000 [Hz]とあるので,通常は1 [ms]周期でセンサ値が更新されます.
また、ETロボコンではよく次のモードが使用されます.

表1.EV3カラーセンサの2つのモードについて

モード 用途 メソッド
反射光モード ライントレース(基本) ev3api::ColorSensor.getBrightness()
RGBモード フィールドやブロックの色推定
ライントレース(応用)
ev3api::ColorSensor.getRawColor()

2018年大会では,私たちのチームはライントレースを反射光モード,フィールド色の特定をRGBモードで行うことにしました.
ライントレースと色の特定を同時に行いたかったので,この2つのモードを高速で交互に切り替えながら使用していたのですがこれがまずかったようです・・・

罠とは...?

カラーセンサのモードが反射光モードとRGBモードとで切り替わる時,約7~8 [ms]のオーバーヘッドが発生します.これは処理のブロックを伴うので,その間PID制御や自己位置推定更新などといった重要な機能を実行することができません.したがって,ロボットの基本的な制御周期である4 [ms]を優に超えてしまうことで走行体の動作が不安定になり,コースアウトをはじめとした誤動作の原因になることがあります.

検証用プログラム

罠を検証するためのプログラムを作成しました.EV3にはカラーセンサのみを接続し,検証結果はBluetooth経由でPCに送信します.
プロジェクト全体のソースはGithubにリポジトリを置いてます

検証1~3では,下に示すループを指定回数だけ繰り返し行い,かかった合計時間を1000で割ることで,Sleep時間を含んだ1測定あたりの平均時間を計算します.上記の罠があるならば,検証3の平均時間は非常に大きくなるはずです.

検証4では,モード切替にかかる時間の平均と標準偏差を測定します.RGBモードから反射光モード,反射光モードからRGBモードの2パターンについて測定しています.

表2.検証1~3のループについて

検証 1ループ ループ回数
検証1 反射光の測定→sleep() 1000回
検証2 RGBの測定→sleep() 1000回
検証3 反射光の測定→sleep()→RGBの測定→sleep() 500回
(app.cpp)
#include "ev3api.h"
#include "app.h"

#include <cmath>

#include "./include/Clock.h"
#include "./include/ColorSensor.h"

#define N_ITE (1000) // 検証用の繰り返し回数.

/* 検証4で使用する配列. メインタスク内で定義すると何故かクラッシュするので仕方なくここに書きます */
int t_bright_to_rgb[N_ITE]; // 反射光モードからRGBモードに切り替わるまでの時間 [ms].
int t_rgb_to_bright[N_ITE]; // RGBモードから反射光モードに切り替わるまでの時間 [ms].

ev3api::Clock *clock;
ev3api::ColorSensor *colorSensor;

void main_task(intptr_t unused) {

    /* new を使用したインスタンス生成 */
    clock = new ev3api::Clock();
    colorSensor = new ev3api::ColorSensor(PORT_2);

    static FILE *p_bt = ev3_serial_open_file(EV3_SERIAL_BT); // Bluetoothファイルハンドル

    /* 初期化処理 */
    clock->reset();

    for (int sleep_duration = 0; sleep_duration <= 10; sleep_duration++) {
        fprintf(p_bt, "Sleep_duration = %d [ms] : \n", sleep_duration);
        /* 検証1 : ev3api::ColorSensor.getBrightness() の検証 */
        {
            int trash_brightness;

            trash_brightness = colorSensor->getBrightness(); // ウォーミングアップのために一度反射光を測定.
            clock->sleep(1000); // 自タスクを1000[ms]スリープ. ウォーミングアップ完了待ちのため.

            // 検証開始.
            int t1 = (int)clock->now(); // 繰り返し実行前のクロック時間 [ms].
            for (int i = 0; i < N_ITE; i++) {
                trash_brightness = colorSensor->getBrightness(); // 反射光を測定.
                clock->sleep(sleep_duration);
            }
            int t2 = (int)clock->now(); // 繰り返し実行後のクロック時間 [ms].

            // 結果をタスクログに表示.
            fprintf(p_bt, "Test_1 : %5d [ms]\n", (t2 - t1));
        }

        /* 検証2 : ev3api::ColorSensor.getRawColor() の検証 */
        {
            rgb_raw_t trash_rgb;

            colorSensor->getRawColor(trash_rgb); // ウォーミングアップのために一度RGB Raw値を測定.
            clock->sleep(1000); // 自タスクを1000[ms]スリープ. ウォーミングアップ完了待ちのため.

            // 検証開始.
            int t1 = (int)clock->now(); // 繰り返し実行前のクロック時間 [ms].
            for (int i = 0; i < N_ITE; i++) {
                colorSensor->getRawColor(trash_rgb); // RGB Raw値を測定.
                clock->sleep(sleep_duration);
            }
            int t2 = (int)clock->now(); // 繰り返し実行後のクロック時間 [ms].

            // 結果をタスクログに表示.
            fprintf(p_bt, "Test_2 : %5d [ms]\n", (t2 - t1));
        }

        /* 検証3 : 上二つの関数を交互に使用する時の検証その1 */
        {
            int trash_brightness;
            rgb_raw_t trash_rgb;

            colorSensor->getBrightness(); // ウォーミングアップのために一度反射光を測定.
            clock->sleep(1000); // 自タスクを1000[ms]スリープ. ウォーミングアップ完了待ちのため.

            // 検証開始.
            int t1 = (int)clock->now(); // 繰り返し実行前のクロック時間 [ms].
            for (int i = 0; i < (int)(N_ITE / 2); i++) {
                trash_brightness = colorSensor->getBrightness(); // 反射光を測定.
                clock->sleep(sleep_duration);
                colorSensor->getRawColor(trash_rgb); // RGB Raw値を測定.
                clock->sleep(sleep_duration);
            }
            int t2 = (int)clock->now(); // 繰り返し実行後のクロック時間 [ms].

            // 結果をタスクログに表示.
            fprintf(p_bt, "Test_3 : %5d [ms]\n", (t2 - t1));
        }

        fprintf(p_bt, "\n");
    }

    fprintf(p_bt, "\n");

    /* 検証4 : 上二つの関数を交互に使用する時の検証その2 */
    {
        int trash_brightness;
        rgb_raw_t trash_rgb;

        colorSensor->getRawColor(trash_rgb); // ウォーミングアップのために一度RGB Raw値を測定.
        clock->sleep(1000); // 自タスクを1000[ms]スリープ. ウォーミングアップ完了待ちのため.

        // 検証開始.
        int t1, t2, t3; // 各タイミングにおける時間 [ms].

        for (int i = 0; i < N_ITE; i++) {
            t1 = (int)clock->now();
            trash_brightness = colorSensor->getBrightness();
            t2 = (int)clock->now();
            colorSensor->getRawColor(trash_rgb);
            t3 = (int)clock->now();

            t_bright_to_rgb[i] = t3 - t2;
            t_rgb_to_bright[i] = t2 - t1;
        }

        // 平均と標準偏差を求めるための計算.
        int t_sum_bright_to_rgb = 0; // 反射光モードからRGBモードに切り替わるまでの時間の合計 [ms].
        int t_sum_rgb_to_bright = 0; // RGBモードから反射光モードに切り替わるまでの時間の合計 [ms].
        int t_square_sum_bright_to_rgb = 0; // 時間の二乗の合計 [ms]. 標準偏差の計算に必要.
        int t_square_sum_rgb_to_bright = 0; // 時間の二乗の合計 [ms]. 標準偏差の計算に必要.

        for (int i = 0; i < N_ITE; i++) {
            t_sum_bright_to_rgb += t_bright_to_rgb[i];
            t_sum_rgb_to_bright += t_rgb_to_bright[i];
            t_square_sum_bright_to_rgb += t_bright_to_rgb[i] * t_bright_to_rgb[i];
            t_square_sum_rgb_to_bright += t_rgb_to_bright[i] * t_rgb_to_bright[i];
        }

        // 平均を求める.
        float t_mean_bright_to_rgb = (float)t_sum_bright_to_rgb / (float)N_ITE;
        float t_mean_rgb_to_bright = (float)t_sum_rgb_to_bright / (float)N_ITE;

        // 2乗平均を求める.
        float t_square_mean_bright_to_rgb = (float)t_square_sum_bright_to_rgb / (float)N_ITE;
        float t_square_mean_rgb_to_bright = (float)t_square_sum_rgb_to_bright / (float)N_ITE;

        // 標準偏差を求める.
        float t_sd_bright_to_rgb = std::sqrt(t_square_mean_bright_to_rgb - std::pow(t_mean_bright_to_rgb, 2.0f));
        float t_sd_rgb_to_bright = std::sqrt(t_square_mean_rgb_to_bright - std::pow(t_mean_rgb_to_bright, 2.0f));

        // 結果をタスクログに表示.
        fprintf(p_bt, "Test_4 (bright to RGB) : \n");
        fprintf(p_bt, "Ave : %5.2f\nSD  : %5.2f\n", t_mean_bright_to_rgb, t_sd_bright_to_rgb);
        fprintf(p_bt, "Test_4 (RGB to bright) : \n");
        fprintf(p_bt, "Ave : %5.2f\nSD  : %5.2f\n", t_mean_rgb_to_bright, t_sd_rgb_to_bright);

    }

    /* タスク終了処理 */
    delete(colorSensor);
    delete(clock);
    ext_tsk();
}

検証結果

検証結果は表3,表4のようになりました.表3から次のようなことがわかります.

  • タスクスリープ時間が0 [ms]の時,検証1と検証2では平均時間はほとんど0 [ms]
    • 最後に計測してから1 [ms]以内にセンサ値を取得する場合,センサは計測を行わず,以前の結果を返す.
  • タスクスリープ時間がxx (>= 1) [ms]以上の時,検証1と検証2では平均時間はxx + 1 [ms]
    • 最後に計測してから1 [ms]以上経過してからセンサ値を取得する場合,センサは計測を行う(図1,図2).
  • 検証3では,検証1,検証2と比較して平均時間が非常に大きい
    • 光センサのモード切り替えに約7 [ms]を消費してしまうため(図3)

つまり,光センサのモード切替が頻繁におこるような設計にしていると,ロボットの制御周期が本来ほしい値よりも非常に大きくなってしまいます.
2018年大会ではこの現象が発生するようなプログラムを書いたため、コースアウトが発生してしまったのだと考えられます.

また,光センサのモード切替が起こらない設計にしていたとしても,光センサの計測に必ず1 [ms]を要することがわかります.したがって,光センサの値を取得するメソッドをメインタスク内に書く(制御周期はsleep()メソッドで制御)のと周期タスク内に書く(制御周期は周期ハンドラで制御)のとで,実際の制御周期が最大1 [ms]変わってきます.周期ハンドラは設定した周期きっかりに動作しますが,sleep()関数では今回の結果からわかるように設定した値より少し長くなってしまいます.

表3.検証1~3の結果

タスクスリープ時間 [ms] 検証1 : 反射光のみを取得したときの平均時間 [ms] 検証2 : RGBのみを取得したときの平均時間 [ms] 検証3 : 反射光とRGBを交互に取得したときの平均時間 [ms]
0 0.006 0.006 7.632
1 2.00 2.00 8.009
2 3.00 3.00 9.245
3 4.00 4.00 10.306
4 5.00 5.00 11.160
5 6.00 6.00 12.267
6 7.00 7.00 13.174
7 8.00 8.00 14.250
8 9.00 9.00 15.144
9 10.00 10.00 16.170
10 11.00 11.00 17.238

表4.検証4の結果

(test4)
Test_4 (bright to RGB) :
Ave :  7.24
SD  :  0.43
Test_4 (RGB to bright) :
Ave :  8.38
SD  :  1.22

ex1.jpg
図1.検証1の時系列(タスクスリープ = 4 [ms])

ex2.jpg
図2.検証2の時系列(タスクスリープ = 4 [ms])

ex3.jpg
図3.検証3の時系列(タスクスリープ = 4 [ms])

ではどうする...?

この罠を避けるためには,光センサのモード切替が発生しないような設計にするのと,制御周期が常に一定になるようにするのが一番手っ取り早いです.つまり,

ライントレースや色判定などは,すべてRGBモードのみを用いて実装する!!

周期タスクを使いこなす!!(周期の設定値やタスク優先度の設計など、沢山の注意点があるので少し大変ですが)

のがよいと思います.

さいごに

今回はEV3カラーセンサが持つ罠について,実際に検証を行い示しました.
たくさんの強いチームがRGBモードだけを使用しているように見えるのは,これが影響しているからな気がします.

この罠を踏んでしまい,大会で悲しい思いをするチームが減ることを祈ります...

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした