ETロボコンにおける自己位置計測（その1、距離計編） with EV3

ETロボコンにおける計測器の活用

　自己位置計測とは、走行体の移動距離や旋回方位などを逐次計測することによって「走行体がコース上の今どこにいるのか」を判別するものです。自己位置計測は「エリア走行」、「速度計」などの他の要素技術にも必要となる土台とも言える要素技術なので、ここで紹介したいと思います。（出場チームごとによって、自己位置計測の手法や計算式は若干の差異がある場合があります）
　以下に用意しているサンプルプログラムは、ETロボコン2016アドバンストクラス走行体 「HackEV」 のトレッド幅に合わせた実装になります。

こちらの写真が、その「HackEV」になります。

[利用開発環境]
・OS：Windows7
・環境：TOPPERS/EV3RT
・言語：C言語

距離計

距離計の概要

　まずは「距離計」について説明します。距離計は「走行体の移動に対して、車輪がどれほど回転したか」という情報を基に、走行体が移動した距離を計測する技術です。もっと具体的な言い方をしますと「車輪の円周に回転量（回転角度）を乗算した結果が走行距離になる」ということです。

　納豆巻きの海苔を剥がしながらコロコロ転がしていく感じを想像してください。納豆巻きの海苔が全て剥がれた時、納豆巻きが転がって移動した距離は巻かれている海苔の幅に等しくなります。この納豆巻きを車輪に置き換えて考えますと、その「海苔の幅」こそが車輪の円周になるのです。

　要するに、車輪の円周と回転角度さえ分かれば、移動距離を計測することが可能なのです。円周は車輪の直径を実際に測ることで計算して出しましょう。

このようにノギスなどで車輪の直径を測りましょう。もちろん定規でもそれなりの精度で距離計を利用することができます。

距離計のサンプルプログラム

　以下に距離計のサンプルプログラムを記述します。このプログラムでは、距離の計測値を更新する関数 Distance_update() を4ms周期で呼び出す前提で記述されています。そして4ms間の移動距離[mm]を毎回足し合わせることによって、これまでの移動距離を計測しています。4ms間隔ごとに納豆巻きから剥がれた海苔を細かく千切っていって、最後に全部繋げている感じとなります。.........そろそろ車輪が納豆巻きに見えてきた頃だと思いますので、この例えはここまでにしましょう。

　このような構造にしているのは、モータの角度に影響を与えることなく（APIの ev3_motor_reset_counts を呼び出すことなく）、距離計の計測値のリセットなどを行えるようにするためです。

以下のサンプルプログラムの利用方法について説明します。利用手順は以下のようになります。

1.プログラム起動時に、初期化関数 Distance_init() を呼び出す（1回のみ呼び出し）
2.タスクの先頭に更新関数 Distance_update() を呼び出す（タスク周期毎に呼び出し）
3.現在の走行距離を得たいときに、距離取得関数 Distance_getDistance() を呼び出す

以上が手順になります。あと、左右タイヤの4ms間の距離を取得する関数（Distance_getDistance4msRight()，Distance_getDistance4msLeft() ）が入っていますが、これは次記事で説明する「方位計測」で用いる関数ですので、ここでは関係ありません。

Distance.c

#include "Distance.h"

#define TIRE_DIAMETER 81.0  //タイヤ直径（81mm）

static float distance = 0.0;     //走行距離
static float distance4msL = 0.0; //左タイヤの4ms間の距離
static float distance4msR = 0.0; //右タイヤの4ms間の距離
static float pre_angleL, pre_angleR; // 左右モータ回転角度の過去値

/* 初期化関数 */
void Distance_init() {
    //各変数の値の初期化
    distance = 0.0;
    distance4msR = 0.0;
    distance4msL = 0.0;
    //モータ角度の過去値に現在値を代入
    pre_angleL = ev3_motor_get_counts(left_motor);
    pre_angleR = ev3_motor_get_counts(right_motor);
}

/* 距離更新（4ms間の移動距離を毎回加算している） */
void Distance_update(){
    float cur_angleL = ev3_motor_get_counts(left_motor); //左モータ回転角度の現在値
    float cur_angleR = ev3_motor_get_counts(right_motor);//右モータ回転角度の現在値
    float distance4ms = 0.0;        //4msの距離

    // 4ms間の走行距離 = ((円周率 * タイヤの直径) / 360) * (モータ角度過去値　- モータ角度現在値)
    distance4msL = ((PI * TIRE_DIAMETER) / 360.0) * (cur_angleL - pre_angleL);  // 4ms間の左モータ距離
    distance4msR = ((PI * TIRE_DIAMETER) / 360.0) * (cur_angleR - pre_angleR);  // 4ms間の右モータ距離
    distance4ms = (distance4msL + distance4msR) / 2.0; //左右タイヤの走行距離を足して割る
    distance += distance4ms;

    //モータの回転角度の過去値を更新
    pre_angleL = cur_angleL;
    pre_angleR = cur_angleR;
}

/* 走行距離を取得 */
float Distance_getDistance(){
    return distance;
}

/* 右タイヤの4ms間の距離を取得 */
float Distance_getDistance4msRight(){
    return distance4msR;
}

/* 左タイヤの4ms間の距離を取得 */
float Distance_getDistance4msLeft(){
    return distance4msL;
}

Distance.h

#ifndef _DISTANCE_H_
#define _DISTANCE_H_

#include "ev3api.h"
#include "parameter.h"

/* 円周率 */
#define PI 3.14159265358

/* 初期化関数 */
void Distance_init();

/* 距離を更新 */
void Distance_update();

/* 走行距離を取得 */
float Distance_getDistance();

/* 右タイヤの4ms間の距離を取得 */
float Distance_getDistance4msRight();

/* 左タイヤの4ms間の距離を取得 */
float Distance_getDistance4msLeft();
#endif

距離計プログラムの実行例（動画）

400mm前進します。

以下が、その実行コード（一部抜粋）となります。

typedef enum {
    AHEAD,
    END
} RUN_STATE;

void main_task(intptr_t unused) {
    static RUN_STATE state = AHEAD;

    /* モーター出力ポートの設定 */
    ev3_motor_config(left_motor, LARGE_MOTOR);
    ev3_motor_config(right_motor, LARGE_MOTOR);

    /* 計測器初期化 */
    Distance_init();
    Direction_init();

    while(1) {
        /* 距離計，方位計 更新 */
        Distance_update();
        Direction_update();

        switch(state) {
        case AHEAD:
            //左右車輪駆動
            ev3_motor_set_power(left_motor, 30);
            ev3_motor_set_power(right_motor, 30);
            //400mm以上前進したら，次状態遷移
            if(Distance_getDistance() > 400.0) {
                state = END;
            }
            break;
        case END:
            //左右車輪停止
            ev3_motor_stop(left_motor, true);
            ev3_motor_stop(right_motor, true);
            break;
        default:
            break;
        }
    }
}