無人駕駛汽車系統入門（四）——反饋控制入門，PID控制

前面幾篇博客介紹了卡爾曼濾波的一些基本算法，其實目標追蹤，定位，傳感器融合還有很多問題要處理，這些我們在以後的系列博客中在進一步細講，現在我想給大家介紹一下無人駕駛汽車系統開發中需要的控制相關的理論和技術，還是和第一篇說的那樣，我想到哪就寫到哪，追蹤和定位等更高級的算法我在後面會繼續寫。所以感興趣的同學可以關注我的博客，無人駕駛汽車系統入門系列博客會一直更新下去。

這一篇主要講控制的入門，爲什麼需要控制理論，以及最經典的PID控制，更高級的應用在實際系統中的控制算法我會在後面的文章中詳述。編寫不易，轉載請註明出處：http://blog.csdn.net/adamshan/article/details/78458325

爲什麼需要控制理論

試想有如下場景，當你駕駛一輛汽車通過這個彎道的時候，假設你已經知道你要開的路線，那麼你會怎麼去操作控制你的車呢？

顯然，如果你不是專業的選手的話，你無法做到一步到位的控制，你需要一邊觀察車輛相對於你想要開的路線的相對偏差，一邊調整你的方向盤的角度和油門踏板的力度，這種基於環境反饋的控制我們稱爲 反饋控制 。反饋控制是現代控制理論的基礎，這是反饋控制的一般思路：

我們希望我們控制的對象（無人車）能夠按照我們希望（規劃好）的路徑行駛，我們會將環境當前給我們的反饋（我們當前的位置）和參考線進行比較，得到我們當前偏離參考線的距離（誤差），基於這個誤差，我們設計一定的算法來產生輸出信號，使得這個誤差不斷的變小，這樣的過程就是反饋控制的一般過程。那麼我們如何基於這個誤差來產生控制指令呢？我們最直觀的感覺就是要讓誤差在我們的控制下逐漸變小直到爲0：

0誤差就意味着車一直在你想讓它開的路徑上開。如何減少誤差就是我們這幾篇博客要向大家介紹的內容。

爲了瞭解反饋控制，我先向大家介紹 PID控制，PID控制是目前利用最爲廣泛的控制理論，我們以它爲出發點討論控制理論。

比例，積分和導數

PID就是指 比例（proportion）、積分（integral）、導數（derivative），這三項表示我們如何使用我們的誤差來產生控制指令，整個流程如下：

首先是根據反饋和參考值求出誤差，這裏的誤差根據具體的情況可以是各種度量，比如說控制車輛按照指定的路徑形式，那麼就是車輛當前位置和參考線的距離，控制車輛的速度在設定的值，那麼就是當前速度和設定速度的差值，求出誤差以後，再根據誤差求比例，積分和微分三項，其中 Kp , Ki , 和 Kd 是三項的係數，它們決定着這三項對最後輸出的影響的比重。將 P,I,D 三項求和作爲最後的輸出信號。我們分別討論這三項的意義。

P控制

考慮一個簡單的情況，假設我們希望無人車按照圖中綠線行駛，但是我們的車在如圖所示的位置：

那麼我們要轉多少度角呢？如果都按照固定的角度轉（如下圖），那麼車的軌跡將如圖中所示：

那麼顯然坐這樣的車是不舒服的。一個直觀的解決方法就是使用比例控制。如圖所示，當偏差大的時候，我們偏轉更多的角度，當偏差小的時候，則偏轉小一點。

那麼這就是P control（比例控制）這裏我們使用 CTE（Cross Track Error） 作爲偏差度量 ，CTE就是我們到參考線的距離。那麼這個時候轉角就變成了：

steering angle=Kp⋅e(t)

其中的 e(t) 就是在t時刻的CTE，在P控制中係數 Kp 會直接影響到實際的控制效果，在合理的數值範圍內 Kp 越大控制的效果越好（越快速的回到參考線附近），但是，當本身位置和參考線相距很遠且 Kp 係數較大的時候，就會出現車輛失去控制的情況：

所以說，如果 Kp 參數設計合理的話，P控制要比固定控制要更好，但是還是不能控制的很好，因爲P控制的車輛容易0值的影響，如圖所示：

此時車輛雖然在參考線上，但是並不是我們希望的狀態（它在下一刻就會偏離），但是對於P控制而言，這是理想狀態，此時控制轉角爲0,因此，P控制會一次又一次的超過參考線（overshot），爲了矯正這種overshot，我們需要考慮一個額外的誤差項——CTE變化率。

PD控制

CTE的變化率描述了我們的無人車向着參考線方向移動的有多快，如果我們的無人車一直都完美的在參考線上運動的話，那麼我們的CTE變化率就爲0。那麼這一項（描述誤差的變化率）就可以用導數來表示，那麼，現在我們的控制輸出就變成了比例項和導數項求和的形式：

steering angle=Kp⋅e+Kdd e(t)dt

其中的 Kd 就是導數項的係數，它的大小決定了CTE變化率對於反饋控制的影響。此時我們的控制叫做PD控制，在PD控制中，我們有兩個係數需要調整，直觀上來看，增大 P 係數將會增大無人車向着參考線方向運動的傾向；增大 D 係數將會增大無人車快速向參考線方向的運動的“抵抗力”從而使得向參考線方向的運動變得更加平滑。使用過大的 P 係數，過小的 D 係數的系統我們稱之爲 欠阻尼的（underdamped），這種情況的無人車將沿着參考線震盪前進，反之，如果P係數過小，D係數過大，那麼我們稱之爲 過阻尼的（overdamped），這將使得無人車要較長的時間才能糾正其誤差。合適地選擇 P ，D 參數可以使無人車能快速回到參考線上的同時很好的維持在參考線上運動。

PD控制似乎已經能夠勝任良好的反饋控制了，但其實還不夠，PD控制器可以保證正常的控制的需求，但是當環境存在擾動的時候，比如說下面這種情況：

車在受力發生輕微偏移以後，由於PD控制中下 P 項傾向於向參考線方向運動，而 D 項則嘗試抵消這種傾向，造成無人車始終都無法沿着參考線運動，這個問題叫做 steady state error 爲了解決這個問題，我們再引入一項—— 積分項。

PID控制

我們將積分項也就如到我們的控制輸出函數中，這個時候，無人車的轉角就可以表示爲：

steering angle=Kp⋅e+Kdd e(t)dt+Ki∫t0e(t)dt

其中 Ki 就是積分項係數，積分項在我們這個例子中其實很好理解，本質就是車的實際路線到參考線的圖形的面積，加入積分項以後，控制函數會儘可能使車輛路線的積分儘可能小（也就是使車輛路線和實際運動參考線之間形成的形狀的面積儘可能小），那麼也就避免了steady state這種情況了。

同樣的，這裏的積分項係數的大小也會影響我們整個控制系統的穩定性，過大的 Ki 會使控制系統“震盪”地運行，過小的 Ki 又會使控制的車輛在遇到擾動以後（處於steady state）要很久才能回到參考線上，這在某些情況下勢必會使車輛處於一個危險的境況。

PID控制就是由這三項共同決定的，還有其他應用於無人駕駛汽車的高級控制算法，但是他們都和我們介紹的PID控制的原理相似。

我們發現其實PID實現確實不難，但是三個係數的選擇卻很難，那麼如何選擇PID係數呢？我們可以在我們的控制循環中通過一定的算法不斷嘗試，下面我提供給大家一種尋找參數的算法：

具體的算法見我的C++代碼實例。

PID C++代碼

pid.cpp

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

//using namespace std;

PID::PID() {}

PID::~PID() {}

void PID::Init(double Kp, double Ki, double Kd) {
    parameter.push_back(Kp);
    parameter.push_back(Ki);
    parameter.push_back(Kd);

    this->p_error = 99999999.;
    this->d_error = 0.0;
    this->i_error = 0.0;

    //twiddle parameters
    need_twiddle = false;

    step = 1;
    // let the car run at first 100 steps, then in the next 3000 steps add the cte^2 to the total_error
    val_step = 100;
    test_step = 2000;

    for (int i = 0; i < 3; ++i) {
        // init the change rate with the value of 0.1*parameter
        changes.push_back(0.1 * parameter[i]);
    }
    index_param = 0;

    best_error = std::numeric_limits<double>::max();
    total_error = 0;
    // fail to make the total_error better times
    fail_counter = 0;
}

void PID::UpdateError(double cte) {
    if(step == 1){
        p_error = cte;

    }
    d_error = cte - p_error;
    p_error = cte;
    i_error += cte;

    if(need_twiddle){
        if(step % (val_step + test_step) > val_step){
            total_error += (cte * cte);
        }

        if(step % (val_step + test_step) == 0){
            std::cout<<"==============  step "<<step<<" =============="<<std::endl;
            std::cout << "P: "<< parameter[0]<<" I: "<<parameter[1]<<" D: "<<parameter[2]<<std::endl;
            if (step == (val_step + test_step)){
                if(total_error < best_error){
                    best_error = total_error;

                }
                parameter[index_param] += changes[index_param];
            } else{
                if(total_error < best_error){
                    best_error = total_error;
                    changes[index_param] *= 1.1;
                    IndexMove();
                    parameter[index_param] += changes[index_param];
                    fail_counter = 0;
                } else if(fail_counter == 0){
                    parameter[index_param] -= (2*changes[index_param]);
                    fail_counter++;
                } else{
                    parameter[index_param] += changes[index_param];
                    changes[index_param] *= 0.9;
                    IndexMove();
                    parameter[index_param] += changes[index_param];
                    fail_counter = 0;
                }
            }

            std::cout << "best_error: "<< best_error<<" total_error: "<<total_error<<std::endl;
            std::cout << "change_index: "<<index_param<<" new_parameter: "<<parameter[index_param]<<std::endl;
            std::cout <<  std::endl;
            total_error = 0;
        }
    }
    step++;
}

double PID::TotalError() {
    return -parameter[0] * p_error - parameter[1] * i_error - parameter[2] * d_error;
}

void PID::IndexMove() {
    index_param++;
    if(index_param >=3){
        index_param = 0;
    }
}

pid.h

#ifndef PID_H
#define PID_H

#include <cmath>
#include <vector>

class PID {
private:
    int step;
    std::vector<double> changes;
    double best_error;
    double total_error;
    int index_param;

    int val_step;
    int test_step;

    int fail_counter;

    void IndexMove();

    bool need_twiddle;

public:
    /*
    * Errors
    */
    double p_error;
    double i_error;
    double d_error;

    /*
    * Coefficients, the order is P, I, D
    */
    std::vector<double> parameter;

    /*
    * Constructor
    */
    PID();

    /*
    * Destructor.
    */
    virtual ~PID();

    /*
    * Initialize PID.
    */
    void Init(double Kp, double Ki, double Kd);

    /*
    * Update the PID error variables given cross track error.
    */
    void UpdateError(double cte);

    /*
    * Calculate the total PID error.
    */
    double TotalError();
};

#endif /* PID_H */

用法

在你的實際控制循環中，調用：

PID pid;
pid.Init(0.3345, 0.0011011, 2.662); //your init parameters

for (in your control loop) {
  pid.UpdateError(cte);
  steer_value = pid.TotalError();
}