0からわかるPID制御（実装編） #C++

今回はC++でPID制御を実装し，シミュレータで自動車を走行させてみようと思います．

問題設定

ある速度で走行している自動車と道路の中心の距離(Cross Track Error:CTE)をハンドル角を制御するだけで小さくすることを目標とします．

PID制御の実装

PID制御の実装部分となります．ヘッダーファイル，メインファイルはこちらを参照してください．

PID制御は以下の式で書くことができます．

steer = K_p cte + K_D \frac{d}{dt}cte + K_I \sum cte

#include "PID.h"
#include <iostream>

using namespace std;

PID::PID() {} //コンストラクタ

PID::~PID() {} //デストラクタ

void PID::Init(double Kp_, double Ki_, double Kd_) {
  Kp = Kp_; //比例ゲイン
  Ki = Ki_; //積分ゲイン
  Kd = Kd_; //微分ゲイン
}

void PID::UpdateError(double cte) {
  if(!init_d){
    init_d = true;
    d_error = cte;
    prev_cte = cte;
  }else{
    d_error = cte - prev_cte; //cteの変化量
    prev_cte = cte;
  }
  p_error = cte; //現在のcteの値
  i_error +=cte; //cteの合計
}

double PID::TotalError() {
  return -Kp*p_error - Ki*i_error - Kd*d_error; //上式のsteer
}

PID制御では
・現在のcte，
・cteの変化量
・cteの合計
を求める必要があり，UpdataErrorの部分で計算をしています．

パラメータチューニング

PID制御では$K_P$,$K_I$,$K_D$の値を設定する必要があります．様々な方法がありますが，試行錯誤で適切な値を探ることにします．

ステップ１

$K_I$,$K_D$の値を0とし，$K_P$のみで制御する．
$K_P$の値が小さければ，自動車の振動は小さいものの，目標に達するまで時間がかかります（本シミュレーションの場合はハンドルを切るのが遅くてコースアウトしてしまいます）．逆に$K_P$の値を大きくしすぎると，自動車が目標値を超えて大きく振動してしまいます．小さすぎず，大きすぎない値を見つける必要があります．

ステップ２

$K_D$に値を入れてみる．次に微分ゲインに数値を入れてみます．$K_D$が小さいとほぼ先ほどと大きくは変わりません．大きくしすぎると，ハンドル角の変化が小さくなりすぎてしまい，制御することできなくなります．適切な値を選ぶと，オーバーシュートが小さくなり，ほぼ目標値に追従して進むようになります．

ステップ３

本シミュレータでは$K_I$を調整しなくても，制御することはできますが，適切な値を入れると，さらに目標値に近づきます．

結果

以上によりパラメータチューニングを終えた結果がこちらとなります．
また，動画では自動車のスピードにもPID制御をしており，95MPHを目標値として設定しています．

ディープラーニングの手法との比較

以前にディープラーニングを用いてこのコースを走行させました．
前回は画像と人間の操作するハンドル角の量を用いてCNNをトレーニングさせたので，本手法と方法が全く異なりますが，比較をしようと思います．

ディープラーニングによる方法

欠点

・滑らかな走行ができていない（ジグザグ走行）．
・高速で走行させようとすると，コースアウトしてしまう．
・トレーニングに時間がかかる

利点

・道路の基準線（車線の中心）を求める必要がない
・未知のコースにでも対応できる

PID制御による手法

欠点

・パラメータチューニングが難しい
・道路の基準線（車線の中心）をあらかじめ求めておく必要がある．

利点

・ディープラーニングの手法よりも滑らかに走行する．
・高速で走行可能

本シミュレータで比較した場合，PID制御の方が実装が簡単でさらに，良い結果も得られました．
ただし，PID制御の場合，CTEを求めるために道路の基準線をあらかじめ求めておく必要があります．
これにはPath Planningという手法を用いなければなりません．一方ディープラーニングでは，Path Planningにより，経路を求めておく必要がありません．

感想

ディープラーニングの技術進化はめざましいため，今後も様々な手法が提案されていくものだと思われます．
PID制御は古典的な方法でありながらも，問題設定が単純なため良い結果が得られました．
自動運転を実現するためにはディープラーニング等の最先端の手法のみならず，PID制御のような古典的な制御方法も必要であるということがわかりました．

本記事ではPID制御により出力目標値が得られてから，その値を実際に入力するまでの間の時間差は今回考慮していません．しかし，実際の自動車に適応する際には，その時間差が少なからずあります（人間は瞬時にハンドルを目標の角度まで切ることはできないため）．その場合，PIDが上手く機能しないことがあります．将来の出力を予測する手法の一つにモデル予測制御というものがあり，次回はモデル予測制御をこのシミュレータに適応してみようと思います．