PID控制

一、PID

1. P Controller

PID控制是控制學中一個很廣的領域。這里首先以 P Controller 的例子講起，首先給定一條規划好的參考軌跡線，為x軸，小車一開始在預定線路的上方，如圖1所示，我們需要讓小車重新行駛回預先規划的線路。

圖1. CTE

轉向與橫切誤差CTE是小車與參考軌跡線的垂直距離。因為我們參考的軌跡線是x軸，所以CTE就是小車的當前y位置。之后，通過規划線路的距離CTE獲得調整的轉向角度 \(\alpha\)，即
\begin{equation}
\alpha = -\tau_p \cdot CTE
\end{equation}

這里的\(\tau_p\)是P Controller比例系數。然后我們調用move使得小車根據獲得的轉向角度開始移動。

def run(robot, tau, n=100, speed=1.0):
   x_trajectory = []
   y_trajectory = []
   # steering = -tau * crosstrack_error
   for i in range(n):
       # move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0)
       crosstrack_error = robot.y - 0
       steering = -tau * crosstrack_error
       robot.move(steering, speed)
       x_trajectory.append(robot.x)
       y_trajectory.append(robot.y)

   return x_trajectory, y_trajectory

robot = Robot()
robot.set(0, 1, 0)  # 初始的坐標(0,1)，方向角0
x_trajectory, y_trajectory = run(robot, 0.2)  # tau = 0.2

最后的小車的運動曲線如圖2所示。雖然小車會向參考軌跡線靠攏，但也沒有預想的那樣直接運行到參考線上,而是在參考線附近不停的震盪，且有失控的趨勢。

圖2. P Controller

2. PD Controller

如果單使用P Controller，會導致軌跡震盪，這是因為P Controller在小車在到達指定軌跡線路時，調整的轉向角度的值沒有立即變為0。為了使調整的轉向角度能隨着\(|CTE|\)的減少而減小角度，我們可以增加一個D Controller。微分的作用主要用於克服被控對象的滯后，在溫度控制系統中也經常使用。在加入D Controller的功能后， \(\alpha\)的表達式如下

\begin{equation}
\alpha = -\tau_p \cdot CTE - \tau_d \frac{d}{dt} CTE
\end{equation}

這里的$\tau_d $是D Controller比例系數。然后我們調用move使得小車根據獲得的轉向角度開始移動。

# steering = -tau_p * CTE - tau_d * diff_CTE
# where differential crosstrack error (diff_CTE)
# is given by CTE(t) - CTE(t-1)
def run(robot, tau_p, tau_d, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    pre_cte = robot.y
    for i in range(n):
        cte = robot.y
        diff_CTE = cte - pre_cte
        pre_cte = robot.y
        steer = -tau_p * cte - tau_d * diff_CTE
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory
    
robot = Robot()
robot.set(0, 1, 0) # 初始的坐標(0,1)，方向角0
x_trajectory, y_trajectory = run(robot, 0.2, 3.0) # tau_p = 0.2 , tau_d = 3.0

最后的小車的運動曲線如圖3所示。小車很快調整到規划的線路，且消除了震盪。

圖3. PD Controller

3. PID Controller

在現實的系統中，總會有一些意料之外的系統偏差引入，這部分累計誤差的消除可以借助 I Controller。積分的作用主要用於消除自動控制系統的剩余誤差，在加入 I Controller的功能后， \(\alpha\)的表達式如下

\begin{equation}
\alpha = -\tau_p \cdot CTE - \tau_d \frac{d}{dt} CTE - \tau_i \sum CTE
\end{equation}

這里的\(\tau_i\)是 I Controller比例系數。然后我們調用move使得小車根據獲得的轉向角度開始移動。

# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    # TODO: your code here
    pre_cte =robot.y
    int_CTE = 0
    for i in range(n):
        CTE = robot.y
        diff_CTE = CTE - pre_cte
        pre_cte = CTE
        int_CTE += CTE
        steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
        robot.move(steering, speed)

        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory

robot = Robot()
robot.set(0, 1, 0)  # 初始的坐標(0,1)，方向角0
robot.set_steering_drift(10.0/180.0*np.pi) # 設定系統偏差
x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.008)   # tau_p = 0.2 , tau_d = 3.0 , tau_d = 0.008

最后的小車的運動曲線如圖4所示。小車雖然一開始因為系統偏差導致越界，但是隨着時間的累積，積分項會糾正系統誤差。而積分項的加入會增加系統控制的魯棒性。

圖4. PID Controller

二、Twiddle算法

Twiddle算法是一種局部爬山算法，其通過遍歷\(\tau_p\)，\(\tau_i\)，\(\tau_d\)所有可能值，以獲得較優的結果，這降低了PID調參的難度。

def twiddle(tol=0.2):
    # Don't forget to call `make_robot` before every call of `run`!
    p = [0, 0, 0]
    dp = [1, 1, 1]
    robot = make_robot()
    x_trajectory, y_trajectory, best_err = run(robot, p)
    #  twiddle loop here
    it = 0
    while(sum(dp) > tol):
        print("Iteration {}, best error = {}".format(it, best_err))
        for i in range(len(p)):
            p[i] += dp[i]
            robot = make_robot()
            x_trajectory, y_trajectory, err = run(robot, p)

            if err < best_err:
                best_err = err
                dp[i] *= 1.1
            else:
                p[i] -= 2 * dp[i]
                robot = make_robot()
                x_trajectory, y_trajectory, err = run(robot, p)

                if err < best_err:
                    best_err = err
                    dp[i] *= 1.1
                else:
                    p[i] += dp[i]
                    dp[i] *= 0.9
        it += 1

    return p, best_err

params, err = twiddle()

twiddle算法與其他控制器的區別比較，如圖5所示。

圖5. 控制器比較圖

三、調參體驗

3.1 口訣

業界有個整定口訣：參數整定找最佳， 從小到大順序查。先是比例后積分， 最后再把微分加。曲線振盪很頻繁， 比例度盤要放大。曲線漂浮繞大彎， 比例度盤往小扳。曲線偏離回復慢， 積分時間往下降。曲線波動周期長， 積分時間再加長。曲線振盪頻率快， 先把微分降下來。動差大來波動慢， 微分時間應加長。理想曲線兩個波， 前高后低四比一。一看二調多分析， 調節質量不會低。

整定步驟：先比例，后積分，再微分。

3.2 實例

經驗湊試法在實踐中最為實用。在調節參數時，必須認真觀察系統響應情況，根據系統的響應情況決定調整那些參數。 以小車為例，首先調節\(\tau_p\)，因為\(\tau_p\)與問題建立線性關系，所以\(\tau_p\)越大,響應越快。然后考慮怎么用積分項消除系統的偏差。最后再調節\(\tau_d\)，以此來減小系統震盪。

1. 起初，我將所有PID系數設置為0.01。
2. 首先，我保持I和D的系數不變，只調整了P的參數，發現當P=1.5時，相對較好。
3. 之后，我調整了D的參數，發現當D =6時，它是比較好的，我可以跑很長的距離。
4. 最后，保持D的參數不變，調整P的參數，當P=0.25時，順利跑完整個路程。

P = 0.25, I = 0.01, D = 6