C言語でEV3開発(20)
超音波センサと安全状態

2021年2月6日

どもです。
前回は、BluetoothでPCとEv3を接続してコマンドの送受信について書きました。
今回は、超音波センサから前方障害物との距離を取得し、それを元に安全状態を判断する、という内容について書きます。

1.超音波センサ

超音波センサを設定、値を取得する際には、EV3RTのAPIを使用します。
詳細は、コチラに記載されているので、省略します。
必要に応じて、参照してください。
超音波センサのHWのスペックは、コチラからダウンロードできるので、それを参照してください。
また超音波センサの特性については、コチラに実測情報が記載されていますので、併せて参考にして下さい。

2.値の処理

上に書いた情報から、超音波センサから取得する値には、ノイズが乗ることが予想されます。
というか、絶対乗ります。
そのため、このノイズを除去する方法を考えます。
ノイズ除去の方法はいくつかありますが、今回は実装が比較的？一番？簡単と考える「移動平均フィルター」を使用します。
…移動平均フィルターについては、これまた詳細の解説は省略します。
必要に応じて、調べてください。
で、今回した移動平均フィルターは、以下の通りです。

#define DISTANCE_AVERAGE_BUF_SIZE   (3)
static int16_t distance_average_buf[DISTANCE_AVERAGE_BUF_SIZE];

extern int16_t distance_sensor_value;

void calc_distance_average(void) {
    int index;
    int32_t distance_sum = 0;

    //Step forward buffer value and set latest valuet to buffer.
    for (index = 0; index < DISTANCE_AVERAGE_BUF_SIZE - 1; index++) {
        distance_average_buf[index] = distance_average_buf[index + 1];
    }
    distance_average_buf[DISTANCE_AVERAGE_BUF_SIZE - 1] =
        distance_sensor_value;

    for (index = 0; index < DISTANCE_AVERAGE_BUF_SIZE; index++) {
        distance_sum += (int32_t)distance_average_buf[index];
    }

    distance_average_value =
        (int16_t)(distance_sum / DISTANCE_AVERAGE_BUF_SIZE);
}

上記コードの各変数の説明をしますと、

distance_average_buf 移動平均フィルター用のバッファ。
サイズは「3」。
distance_sensor_value 超音波センサから読み出した、障害物との距離。

です。
また、以下にコードの内容を記載します。

  • 最初のforループ→バッファ内のデータを、1つ前に異動する。
  • 最初のforループの次→最新の超音波センサのデータをバッファに格納する。
  • 2つ目のforループ以降→バッファのデータの総和を求め、その後平均値を算出。

これで、移動平均フィルターは完了です。

3.安全状態

上で計算した障害物との距離を元に、安全状態を判定します。
状態としては、

  • 安全
  • 注意
  • 危険
  • 停止

の4つを設定します。
このとき、状態が変化する際には、「不感帯」を設けます。
たとえば、「安全」から「注意」へ状態が変化する際の境界値と、「注意」から「安全」へ状態が変化する際の境界値を、異なる値に設定します。
このようにすることで、安全状態がめまぐるしく変化するような現象の発生を回避します。
距離センサーから取得した値と不感帯、および対応する安全状態の変化の関係を図にすると、以下のようになります。
距離と安全状態
この状態変化に対応した実装を、以下に示します。

const int8_t CAR_SAFE_STATE_SAFE = 0;
const int8_t CAR_SAFE_STATE_ATTN = 1;
const int8_t CAR_SAFE_STATE_DANG = 2;
const int8_t CAR_SAFE_STATE_STOP = 3;

static const int16_t CAR_SAFE_STATE_SAFE_ATTN_BORDER = 200;  //安全→警告
static const int16_t CAR_SAFE_STATE_ATTN_SAFE_BORDER = 210;  //警告→安全
static const int16_t CAR_SAFE_STATE_ATTN_DANG_BORDER = 140;  //注意→危険
static const int16_t CAR_SAFE_STATE_DANG_ATTN_BORDER = 150;  //危険→注意
static const int16_t CAR_SAFE_STATE_STOP_DANG_BORDER = 50;   //停止→危険
static const int16_t CAR_SAFE_STATE_DANG_STOP_BORDER = 60;   //危険→停止

void judge_dist_safe(void) {
    int16_t distance_average_value_tmp;
    
    //Latch latest value.
    distance_average_value_tmp = distance_average_value;

    if (CAR_SAFE_STATE_SAFE == distance_safe_state) {
        if (CAR_SAFE_STATE_SAFE_ATTN_BORDER > distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_ATTN;
        }
    } else if (CAR_SAFE_STATE_ATTN == distance_safe_state) {
        if (CAR_SAFE_STATE_ATTN_DANG_BORDER > distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_DANG;
        }
        if (CAR_SAFE_STATE_ATTN_SAFE_BORDER < distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_SAFE;
        }
    } else if (CAR_SAFE_STATE_DANG == distance_safe_state) {
        if (CAR_SAFE_STATE_DANG_STOP_BORDER > distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_STOP;
        }
        if (CAR_SAFE_STATE_DANG_ATTN_BORDER < distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_ATTN;
        }
    } else if (CAR_SAFE_STATE_STOP == distance_safe_state) {
        if (CAR_SAFE_STATE_STOP_DANG_BORDER < distance_average_value_tmp) {
            distance_safe_state = CAR_SAFE_STATE_DANG;
        }
    }
}

このようにして、距離から安全状態を判定します。

4.まとめ

今回のエントリーでは、超音波センサから取得した障害物との距離を元に、安全状態を判定する内容を記載しました。
これは、Ev3で組み立てていく車の安全制御に関わる部分になります。

ではっ！

ev3,開発

Posted by CountrySideEngineer