こんにちは．kawanon868と申します．
DSPシステム部というチーム名で，ETロボコンのデベロッパー部門アドバンストクラスに参加しています．
戦績として，2018年地区大会では競技成績 4/6位（L, Rコースともにゴールゲート通過後のライントレースでコースアウト）と惨敗を喫しましたが，2019年地区大会では念願の競技成績優勝を果たしました（どちらもアドバンストクラス）．
2018年大会終了後にコースアウトの原因を調査したところ，**EV3 カラーセンサ（カラーセンサー）がもつ仕様書に記載のない隠れた特性（罠）**が原因でした．今回はその罠について検証したいと思います．
この記事は2019年大会で使用された実機のロボットに関するものです．2020年大会のシミュレータでは異なる挙動になる可能性が非常に高いのでご了承ください．
さらに，今回記載するプログラムはEV3RT β7-3用のプログラムです，2020年大会で使用されているV1.0とは仕様がかなり異なるので，そのまま実行は不可能です．
EV3 カラーセンサについて
EV3 カラーセンサはLEGO Education社のカラーセンサで，同社製品であるMindstorms EV3に接続して使用できます．
主な仕様はETロボコンおなじみのAfrel様がまとめている通りです．
サンプリングレートが1,000 [Hz]とあるので，通常は1 [ms]周期でセンサ値が更新されます．
また、ETロボコンではよく次のモードが使用されます．
表１．EV3カラーセンサの２つのモードについて
| モード | 用途 | メソッド |
|---|---|---|
| 反射光モード | ライントレース（基本） | ev3api::ColorSensor.getBrightness() |
| RGBモード | フィールドやブロックの色認識 ライントレース（応用） |
ev3api::ColorSensor.getRawColor() |
2018年大会では，私たちのチームはライントレースを反射光モード，フィールド色の特定をRGBモードで行うことにしました．
ライントレースと色の特定を同時に行いたかったので，この２つのモードを高速で交互に切り替えながら使用していたのですがこれがまずかったようです・・・
罠とは...?
カラーセンサのモードが反射光モードとRGBモードとで切り替わる時，**約7~8 [ms]**のオーバーヘッドが発生します．これは処理のブロックを伴うので，その間PID制御や自己位置推定更新などといった重要な機能を実行することができません．したがって，ロボットの基本的な制御周期である4 [ms]を優に超えてしまうことで走行体の動作が不安定になり，コースアウトをはじめとした誤動作の原因になることがあります．
検証用プログラム
罠を検証するためのプログラムを作成しました．EV3にはカラーセンサのみを接続し，検証結果はBluetooth経由でPCに送信します．
プロジェクト全体のソースはGithubにリポジトリを置いてます．
もしこのプログラムを試して結果が得られた方は結果をコメントで教えていただけると嬉しいです！（他の走行体でも同様の結果となるのかを知りたいのです・・・）
検証１～３では，下に示すループを指定回数だけ繰り返し行い，かかった合計時間を1000で割ることで，Sleep時間を含んだ1測定あたりの平均時間を計算します．上記の罠があるならば，検証３の平均時間は非常に大きくなるはずです．
検証４では，モード切替にかかる時間の平均と標準偏差を測定します．RGBモードから反射光モード，反射光モードからRGBモードの2パターンについて測定しています．
表２．検証１～３のループについて
| 検証 | 1ループ | ループ回数 |
|---|---|---|
| 検証1 | 反射光の測定→sleep() | 1000回 |
| 検証2 | RGBの測定→sleep() | 1000回 |
| 検証3 | 反射光の測定→sleep()→RGBの測定→sleep() | 500回 |
#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].
// 結果をBluetooth経由で送信.
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].
// 結果をBluetooth経由で送信.
fprintf(p_bt, "Test_2 : %5d [ms]\n", (t2 - t1));
}
/* 検証3 : 上二つの関数を交互に使用する時の検証その1 */
{
int trash_brightness;
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 < (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].
// 結果をBluetooth経由で送信.
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));
// 結果をBluetooth経由で送信.
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();
}
検証結果
検証結果は表３，表４のようになりました．
表３．検証１～３の結果
| タスクスリープ時間 [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 |
表４．検証４の結果
Test_4 (bright to RGB) :
Ave : 7.24
SD : 0.43
Test_4 (RGB to bright) :
Ave : 8.38
SD : 1.22
表３から次のようなことがわかります．
- タスクスリープ時間が0 [ms]の時，検証１と検証２では平均時間はほとんど0 [ms]
- 最後に計測が行われた後，すぐにセンサ値を再度取得する場合，センサは計測を行わず，以前の結果を返す．
- タスクスリープ時間がxx (>= 1) [ms]以上の時，検証１と検証２では平均時間はxx + 1 [ms]
- 最後に計測が行われてから1 [ms]以上経過してからセンサ値を取得する場合，センサは計測を行う（図１，図２）．
- センサの計測は1 [ms]を要し，処理のブロックを伴う
- 検証していないが，設定周期yy [ms]の周期タスクに置き換えた場合は平均時間はyy [ms]になるはず（後述）
- 検証３では，検証１，検証２と比較して平均時間が非常に大きい
- 光センサのモード切り替えに約7 [ms]を消費してしまうため（図３）
- しかも処理のブロックを伴う
つまり，光センサのモード切替が頻繁におこるような設計にしていると，ロボットの制御周期が本来ほしい値よりも非常に大きくなってしまいます．
2018年大会ではこの現象が発生するようなプログラムを書いたため、コースアウトが発生してしまったのだと考えられます．
また，光センサのモード切替が起こらない設計にしていたとしても，光センサの計測に必ず1 [ms]を要することがわかります．したがって，光センサの値を取得するメソッドをメインタスク内に書く（制御周期はsleep()メソッドで制御）のと周期タスク内に書く（制御周期は周期ハンドラで制御）のとで，**実際の制御周期が1 [ms]変わってきます．**周期ハンドラは設定した周期きっかりに動作しますが，sleep()関数では今回の結果からわかるように設定した値より少し長くなってしまいます．
図１．検証１の時系列（タスクスリープ = 4 [ms]）
図２．検証２の時系列（タスクスリープ = 4 [ms]）
図３．検証３の時系列（タスクスリープ = 4 [ms]）
ではどうする...?
この罠を避けるためには，光センサのモード切替が発生しないような設計にするのと，制御周期が常に一定になるようにするのが一番手っ取り早いです．つまり，
ライントレースや色判定などは，すべてRGBモードのみを用いて実装する！！
周期タスクを使いこなす！！（周期の設定値やタスク優先度の設計など、沢山の注意点があるので少し大変ですが）
のがよいと思います．
余談
各種センサ値や走行体の状態（位置、速度など）をBluetooth経由で監視しようとしたときにやらかしがち
走行がまったく安定しなくなり、PIDパラメータが悪いと勘違いして変な変更がどんどん追加され・・・
さいごに
今回はEV3カラーセンサが持つ罠について，実際に検証を行い示しました．
たくさんの強いチームがRGBモードだけを使用しているように見えるのは，これが影響しているからな気がします．
この罠を踏んでしまい，大会で悲しい思いをするチームが減ることを祈ります...