ETロボコン PID制御でライントレース挑戦してみた​

ETロボコンとは

ETロボコン（Embedded Technology Robot Contest）は、組込みソフトウェア技術の学習を目的とした教育型ロボットコンテストです。
学生だけでなく社会人も参加でき、単純にタイムを競うだけではなく、「モデル（設計）」と「走行（実装）」の両方が評価対象になることが大きな特徴です。参加者のレベルに応じて複数のクラスが用意されています。

シミュレータ部門 エントリークラス

プログラミングや設計の未経験者向けのクラスです。シミュレータ上で開発を行うため、ロボット実機がなくても学習できます。

フィジカル部門 プライマリークラス

プログラミングや設計の基礎知識を持つ方向けのクラスです。実機を用いた開発を通じて組込み技術を学べます。

フィジカル部門 アドバンストクラス

基礎を習得した参加者向けの上級クラスです。より高度なアルゴリズムや設計技術が求められます。

ライントレースとは

ライントレースとは、コース上に描かれた黒線（または白線）をセンサーで検知し、そのラインに沿ってロボットを自動走行させる技術です。

ライントレース走行

この技術はロボット競技だけでなく、工場の搬送ロボットや教育用ロボットなど、さまざまな分野で活用されています。
ETロボコンではカラーセンサーを使用してコースの明暗を検知し、ラインとの位置関係を判断しながら走行します。

ETロボコンセンサー

制御方法の種類

ライントレースにはさまざまな制御方法がありますが、代表的なものとして「ON/OFF制御」「P制御」「PID制御」があります。

ON/OFF制御

最もシンプルな制御方法です。
ラインから外れた場合に大きく方向転換し、ライン上に戻ったら真っ直ぐ進むという動作を繰り返します。
実装は容易ですが、ロボットの動きがガタガタしやすく、高速走行には向いていません。

P制御（比例制御）

現在のラインからのズレ量に応じて旋回量を変化させる方法です。
ズレが大きければ大きく曲がり、ズレが小さければわずかに曲がるため、ON/OFF制御よりも滑らかな走行が可能になります。ただし、急カーブではラインを見失ったり、振動が発生したりすることがあります。

PID制御

今回採用したのがPID制御です。
PID制御は、
P（Proportional：比例）
I（Integral：積分）
D（Derivative：微分）
の3つを組み合わせた制御方式です。P制御で現在のズレに応じた修正を行い、I制御で長時間続くズレを補正し、D制御でズレの変化を予測して先回りした制御を行います。
イメージとしては、
P：ズレた分だけ曲がる
I：ズレが続いたら補正を強める
D：ズレ始めた瞬間にブレーキをかける
という役割になります。
PID制御は「Pで動かし、Dで安定させ、Iで微調整する」と考えると分かりやすいでしょう。 今回のライントレースでは、このPID制御を利用して安定した走行を目指しました。

PID 基本式

ライントレース アルゴリズム

実際のライントレース処理は、以下の流れで実行しています。

①カラーセンサーの値を取得する

まずカラーセンサーからRGB値を取得します。取得したRGB値は、そのままでは扱いにくいため、グレースケール値へ変換します。

mRGB = pup_color_sensor_rgb(gSettingSPIKE.fg_color_sensor);

mGlayscale =
(uint16_t)((((double)mRGB.r)/4) * 0.3 +(((double)mRGB.g)/4) * 0.59 +(((double)mRGB.b)/4) * 0.11); 

人間の視覚特性を考慮した重み付けを行い、RGB値を1つの明るさ情報へ変換しています。

②ライン位置（偏差）を計算

次に、白と黒の中間値を目標値として設定し、現在の輝度との差を計算します。例えば白の輝度が203、黒の輝度が10の場合、
(203 + 10) ÷ 2 = 106.5
が目標値となります。
ロボットはこの値を維持するように制御され、白側へ寄れば右へ、黒側へ寄れば左へ旋回することでラインを追従します。

PID 基本式

③ライン位置を算出しPID制御で旋回量を求める

偏差が求まったら、PID制御を用いて旋回量を計算します。今回使用したPIDゲインは以下の通りです。

#define KP_L 0.5
#define KI_L 0.032
#define KD_L KI_L * 2.5 

プログラムでは、前回の偏差と今回の偏差から比例項（P）、積分項（I）、微分項（D）を計算し、それらを合算して旋回量を求めています。

mIntegral_black += ((mDiff_black[0] + mDiff_black[1]) / 2.0) * DELTA_T;

P_cal = p_para * mDiff_black[1];
I_cal = i_para * mIntegral_black;
D_cal = d_para * ((mDiff_black[1] - mDiff_black[0])) / DELTA_T;

pid_black_value_return = P_cal + I_cal + D_cal; 

また、計算結果が極端な値にならないよう、旋回量には上限値を設けています。このようにして求めた旋回量をもとに、ロボットがラインから外れないよう制御を行います。

④モーター出力を計算する

PID制御によって旋回量が求まったら、その値を左右モーターの出力へ反映します。

left_motor_power  = (int)(gSettingSPIKE.gMax_speed +(gSettingSPIKE.gMax_speed * turn_amount / 100));

right_motor_power = (int)(gSettingSPIKE.gMax_speed - (gSettingSPIKE.gMax_speed * turn_amount / 100)); 

左モーターと右モーターの出力差を変化させることでロボットを旋回させます。
例えば右へ曲がりたい場合は左モーターの出力を大きくし、左へ曲がりたい場合は右モーターの出力を大きくします。 この差動制御によって、ライントレースに必要な滑らかな旋回を実現しています。

⑤10ms～20ms周期で繰り返す

ここまでの処理を一定周期で繰り返し実行します。
ライントレースの基本アルゴリズムでは、

1. センサー値（RGB）を取得する
2. グレースケールへ変換する
3. ライン位置（偏差）を算出する
4. PID制御で旋回量を求める
5. モーター出力を計算する

という一連の処理を10〜20ms周期で繰り返します。ロボットは走行中も常にセンサー値を監視し続けているため、ライン位置の変化に素早く反応することができます。

キャリブレーション

ライントレースを行う前に、カラーセンサーのキャリブレーションを実施します。
キャリブレーションとは、コース上の白・黒・青それぞれの輝度値を実際に測定し、その値をプログラムへ反映する作業です。

黒輝度値を測定

例えば今回の環境では以下の値となりました。

const uint16_t WHITE_BRIGHTNESS = 203;
const uint16_t BLUE_BRIGHTNESS  = 42;
const uint16_t BLACK_BRIGHTNESS = 10; 

センサー値は会場の照明環境やコースの状態によって変化します。 そのため、事前に正確な値を取得しておくことが安定走行の重要なポイントとなります。

チューニング方法

PID制御は実装しただけでは十分な性能を発揮できません。 実際にロボットを走行させながら、各ゲインを調整する必要があります。

Pゲイン

Pゲインを大きくすると曲がる力が強くなります。
しかし大きくしすぎるとラインを行き過ぎてしまい、左右に蛇行する現象が発生します。逆に小さすぎるとカーブへ対応できず、ラインアウトしやすくなります。
今回使用した値は以下です。

#define KP_L 0.5 

Iゲイン

Iゲインはズレが長時間続く場合に補正を強めるための値です。長いカーブやセンサー誤差の補正に有効です。

#define KI_L 0.032 

Dゲイン

Dゲインはズレの変化速度に反応します。
曲がり始めのタイミングで制御を強めることで、振動やオーバーシュートを抑制できます。
今回のプログラムではIゲインを基準に設定しました。

#define KD_L (KI_L * 2.5) 

モーター出力の調整

PIDゲインだけでなく、走行速度も重要な要素です。

#define MinPower_LAP 40
#define MaxPower_LAP 80 

速度を上げるとタイムは向上しますが、その分だけ制御も難しくなります。
そのため、

1. Pゲインを調整する
2. Dゲインで振動を抑える
3. Iゲインで微調整する
4. 最後に速度を上げる

という順番で調整を進めると比較的スムーズにチューニングできます。
実際には何度も試走を繰り返しながら、蛇行しない範囲で最も速く走行できるポイントを探していきました。