ETロボコンにおける自己位置の応用（エリア分割） with EV3

#エリア分割走行
##エリア分割走行とは
　「エリア分割走行」とは、ロボットの走行範囲を「エリア」という小単位区間に分割して区切ることで管理し、その区間を順に切り替えながらロボットが走行区間を移動する技術をいいます。
　この技術を用いてうれしい点としては、そのエリアごとに異なるパラメータ値に基づいて走行できるという点になります。具体的な例を挙げれば、コース上で「カーブ入口では速度を下げる」、「直線部分では速度を上げる」などといった微調整が可能になるのです。
　この技術を用いるためには、まずは走行するコースの寸法が分からないといけません。ですので、エリア分割走行を実装するにあたっては、まず、コース上のどこで区切るのかを決めて、その部分の寸法を計測することから始めます。これを「コース分析」と呼びます。私達が行っているコース分析手法としては、本番コースのPDFデータから距離を計測する手法を用いています。
##コース分析
###コース分析の概要
　コース分析を行う手法は様々あります。それこそ参加チームそれぞれで微妙に異なる手法で計測しているのではないでしょうか。よって、ここでは私が所属しているチーム（Monolith，東北地区所属）で行われている手法を説明します。
###コース分析の手順
　手順としては以下の通りとなります。

1. コースのPDFデータを用意する
   　コースのPDFデータはETロボコンの参加チーム専用ページにて公開されています。このPDFデータをもとに計測作業を行うので、ダウンロードしておいてください。

2. コースを任意のエリアに分割して区切る
   　コースをエリアで分割するために、どの位置でエリアを区切るかを決めます。基本は直線部分とカーブ部分とを切り分けていきます。詳細は下記の「エリアの分け方」の項目にて記述します。

3. エリアごとの距離を計測する
   　エリアを区切ったら、その区切ったエリアの区間距離を計測します。私達のチームでは「Visio」を使っていますが、ここではより簡単に計測できる「Acrobat DC」を使った手法を紹介します。詳細は下記の「エリアの区間距離計測」の項目にて記述します。

###エリアの分け方
　カーブ部分は大体一定の曲率（単一の円で構成されているカーブ）の場合が多いため、直線とカーブの円が接している位置を基準に区切るといいでしょう。下のコース画像（図1）は「Visio」を使って作製したものですが、円の図形と合わせるだけならば、ペイントでもPowerPointでもなんでも結構です。（ただVisioの方が、後述の距離計測や、曲率計測が簡単に行えるのでお勧めです）

図1：コースのエリア分割図（単一の円）

　しかし、ETロボコン2016コースでのカーブの中には、複数の曲率をもつカーブ（複数の円で構成されたカーブ）がありました。こういったカーブは、構成されている円の曲率に合わせてカーブ内を区切ると良いでしょう（図2）。

図2：コースのエリア分割図（複数の円）

　といっても、あまりエリアの数が多くなってしまうと、あとで調整が大変になってしまうので、できる限りエリアの数は少なくなるように区切るのがいいでしょう。

###エリアの区間距離計測
　ここでは「Acrobat DC」内のツールで用意されている「ものさしツール」を利用して、エリアの区間距離を計測していきます。また、この測定方法で表示された数値はPDFデータ上での長さでしかないので、実際のコースでの長さを出すためにも、PDFデータの縮尺を考慮して、実寸距離を計算してください。（ちなみにETロボコン2016に公開されていた本番コースPDFデータは1/2縮尺でした）
　それでは、まずコースのPDFデータを「Acrobat DC」で開きます。その後、「ツール」タブをクリックします。

ツールの一覧からものさしを選択します。

測定したい部分に合わせて「測定タイプ」を選択します。直線距離を測定したい場合は、一番左の「距離ツール」を選択してください。このタイプでは、二点間の距離が表示されます。（図中で表示されている距離は、ツールの矢印が見えやすいように、私が個人的に寸法を色々と調節した結果として表示されている数値なので、実際のコース上の距離とは異なります）

　カーブの距離を測定するためには「周辺ツール」を使います。これは複数点を指定して、その点間の距離の合計を表示するツールです。

　
　以上により、各エリアの区間距離を測定し終えたら、コース分析は終了です。

##エリア分割走行の実装
　エリア分割走行の実装方法ですが、簡単に言ってしまえば「走行距離から現在エリア位置を判別して、その現在位置のエリア情報に基づいて走行する」というだけです。とはいえ、この「エリア分割走行」には決まった形の実装方法があるわけではありませんので、以下に示すサンプルプログラムは「極力まで簡略化したエリア分割走行」の実装一例として用意しました。
###サンプルプログラムの解説
　今回のサンプルプログラムはデモ用として「ロボットが進んだ距離に応じてエリアの切り替えを実行し、現在走行中のエリア情報をLCD（EV3の液晶画面）に表示させる」という処理が記述されています。実行の際は、HackEVの車体を持ち上げて、タイヤの両輪を回してみてください。画面の表示が変わるはずです。

　まずは、エリア情報周辺の解説をします。今回管理しているエリア情報は「エリアの距離」「エリア内での走行速度」「エリア内でのP，I，D係数」の五つになります。サンプルプログラム上では _Area_struct 型の配列 Area_Info[] の中に格納されています。ここで格納されているエリア情報は適当に数値を入力したダミーデータとなります。
　なお、このようにエリアを配列として管理する記述形式は、本当はあまり良い形式ではありません。今回はデモ用としてエリアを7つまでしか用意しませんでしたが、実際の走行ではそれ以上の数のエリアを管理することになります。そういった場合において、配列でエリアを管理してしまうと、長い行にわたってエリア情報を記述しなくてはならず、プログラムが読みにくくなります。さらにいうと、パラメータ値の調整作業の際にも微調整の度にビルドしなくてはならず、非常に面倒です。よってバグの温床にもなるので、余裕があればcsvファイルなどにまとめて管理するなどの工夫をしましょう。

　続いて、エリア情報内の各パラメータ値について説明します。
　「エリアの距離」とは、先程のコース分析で計測した「エリアの長さ」になります。ここではミリメートル単位で管理しています。
　「エリア内での走行速度」とは、そのエリア内では、どの程度の速さで走行するのかを示したパラメータ値になります。ここではモータに送る制御値として0～100の範囲の値で速度を制御します。本当であれば、ここは速度計などの関数を自作して、速度単位のパラメータ値でもって制御するのが一番なのですが、複雑になるので割愛します。
　「P，I，D係数」について知らないという場合は、「車体が綺麗に黒線に沿って走るための値」と思ってください。長くなってしまうので、ここでは詳しく説明いたしません。

　続いて、Area.cでは「エリアの更新」、「エリア情報の取得」の関数が記述されています。
　エリアの更新では、現在の走行距離とエリアの区間距離とを比較し、その区間距離まで走行したら、次のエリアに切り替えるという処理を行っています。なお、ここではエリアの切り替えの度に距離計をリセットしています。この処理は、長距離の走行中に蓄積されていく距離計の誤差をリセットするために行っています。
　app.cでは、主にLCDにエリア情報を出力させる処理が記述されています。エリアの切り替えが発生したら、LCDへの出力を実行するような処理となっています。そのほかの部分は、それぞれの更新を行う関数を呼び出しているだけです。
　なお、ここで利用されている計測器の関数は、私の過去の投稿記事で解説した計測器を利用しています。

###サンプルプログラム

Area.c

#include "Area.h"

#define AREA_INFO_AMOUNT 7

/*エリア情報*/
_Area_struct Area_Info[AREA_INFO_AMOUNT] = {
   /* distance, speed, P, I, D */
	{100.0 , 100, 0.2, 0.4, 0.2}, /*0 直線*/
	{300.0 ,  50, 0.4, 0.4, 0.3}, /*1 右カーブ*/
	{100.0 ,  40, 0.2, 0.4, 0.1}, /*2 直線*/
	{200.0 , 100, 0.4, 0.2, 0.5}, /*3 2本橋*/
	{300.0 ,  70, 0.4, 0.3, 0.6}, /*4 直線*/
	{100.0 ,  80, 0.5, 0.3, 0.2}, /*5 左カーブ*/
	{200.0 , 100, 0.4, 0.2, 0.9}, /*6 直線*/
};

static int Area_curArea = 0; //現在走行中のエリア番号

/*********************************************
エリア情報を「更新するかの判定」と「更新の実行」を行う関数
true：エリア情報を更新する
false：エリア情報を更新しない
**********************************************/
bool_t Area_update() {
	float Area_curDistance = 0;

	Area_curDistance = Distance_getDistance();

	if( Area_curDistance >= Area_Info[Area_curArea].distance ){
		Area_curArea++;
		Distance_init();
		return true;
	}

	return false;
}

/**********************************
エリア情報を取得する関数
***********************************/
_Area_struct Area_getAreaInfo() {
		return Area_Info[Area_curArea];
}

Area.h

#ifndef _AREA_H_
#define _AREA_H_

#include "ev3api.h"
#include "Distance.h"

typedef struct Area_struct {
    float distance;
    float speed;
    float p;
    float i;
    float d;
} _Area_struct;

/*********************************************
エリア情報を「更新するかの判定」と「更新の実行」を行う関数
true：エリア情報を更新する
false：エリア情報を更新しない
**********************************************/
bool_t Area_update();

/**********************************
エリア情報を取得する関数
***********************************/
_Area_struct Area_getAreaInfo();

app.c

#include "ev3api.h"
#include "app.h"
#include "Distance.h"
#include "Direction.h"
#include "Area.h"

#include <stdio.h>
#include <stdlib.h>

#if defined(BUILD_MODULE)
#include "module_cfg.h"
#else
#include "kernel_cfg.h"
#endif

#define DEBUG

#ifdef DEBUG
#define _debug(x) (x)
#else
#define _debug(x)
#endif

/* LCD周辺定義（特に気にしなくてもよい） */
#define CALIB_FONT (EV3_FONT_SMALL)
#define STR 5
#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)

void main_task(intptr_t unused) {
	char str[STR][30];   // LCD出力用文字列変数
	float cur_dir = 0.0; // 方位計の現在値
	float cur_dis = 0.0; // 距離計の現在値
	_Area_struct cur_areaInfo; // 現在走行中のエリア情報

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

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

	while(1) {
		// 計測器更新 
		Distance_update();
		Direction_update();

		// 計測器の現在値を格納
		cur_dis = Distance_getDistance();
		cur_dir = Direction_getDirection();

		// エリア更新 -> 更新されたらLCDに出力
		if(	Area_update() == true) {
			// ここからLCDに出力する処理（中身は気にしなくても良い）
			cur_areaInfo = Area_getAreaInfo();
			ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
			sprintf(str[0], ".distance = %d", (int)cur_areaInfo.distance );
			sprintf(str[1], ".speed = %d"   , (int)cur_areaInfo.speed );
			sprintf(str[2], ".p = %d"       , (int)(cur_areaInfo.p * 10) );
			sprintf(str[3], ".i = %d"       , (int)(cur_areaInfo.i * 10) );
			sprintf(str[4], ".d = %d"       , (int)(cur_areaInfo.d * 10) );
			ev3_lcd_draw_string(str[0], 0, CALIB_FONT_HEIGHT*2);
			ev3_lcd_draw_string(str[1], 0, CALIB_FONT_HEIGHT*3);
			ev3_lcd_draw_string(str[2], 0, CALIB_FONT_HEIGHT*4);
			ev3_lcd_draw_string(str[3], 0, CALIB_FONT_HEIGHT*5);
			ev3_lcd_draw_string(str[4], 0, CALIB_FONT_HEIGHT*6);
			// ここまでLCDに出力する処理（中身は気にしなくても良い）
		}
		
	}
}

　エリア分割走行の解説は以上となります。このエリア分割走行の技術は、ETロボコンに出場するにあたって、多くのチームが工夫して実装している「キーポイント」ともいえる技術なので、まだ実装したことがないという場合は、コース分析と合わせて是非挑戦してみてください。