PID control

|—平滑化算法

|—PID控制—|—P控制器編程

                     |—PD控制編程

                     |—PID控制編程

                     |—參數優化

                     |—實驗P、PD、PID對減少系統偏差的做用

這裏討論怎麼將路徑轉變成行動指令(生成平滑的路徑)，用到PID去控制。

從出發點到目的地，實際的路徑不能像以前的path plan(藍色)，沒有car能立轉90度，也不能像綠色斜轉45度，紅色路徑最好溫和地移動。

 

平滑化算法

將路徑規劃找出來的每一個小格點設爲x₀~x_i~ xn-1（未平滑化），並設置值與xi相等的變量yi，而後作兩項優化使每項平方差最小

if只作第一項優化，結果仍會是原始路徑由於步驟1已經設置的是兩變量相減爲0，而if只作第二項優化會使得相鄰平滑點愈來愈近，最後除了一個原始點get nothing。

能夠看出這兩項式子是互相鬥爭的，使y i更接近xi點意味着路徑趨向於塊狀而不光滑（如上面的藍色路徑）；使y i彼此靠近使得路徑更平滑，但更不真實於原始路徑（像以上綠色路徑）。

 

那如何作到使兩個式子都最小化？能夠經過最小化一個線性組合使得兩個式子都最小化，

c1相對c2越大就越平滑，相反就越接近一個原點，找到這兩個鬥爭的平衡點就能獲得如上面紅線同樣的平滑路徑。

 

模擬情形說明兩項優化後路徑爲何會平滑：

當把y移動遠離x(可對除起始目的點的其餘全部y點)，產生y彼此之間距離越近更平滑的新的路徑，同時第二個式子偏差變小，但新的路徑會使得第一個式子偏差變大，但依靠權重c1和c2的設置能獲得理想的路徑，這個優化只優化中間的點，起始點和目的地點x,y一直相等。

 

如何作？

使用梯度降低，每一次迭代都沿着能減少偏差的方向邁一小步，即每一步都對yi（除y0和yn-1）進行調整使得組合式子最小化：（這個算法紅色部分本來是-，但這個模型裏是+）

make code：

from math import *
path = [[0, 0],
        [0, 1],
        [0, 2],
        [1, 2],
        [2, 2],
        [3, 2],
        [4, 2],
        [4, 3],
        [4, 4]]
def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001):#tolerance是偏差容忍變量 

#將路徑深度複製到newpath
    newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))]
    for i in range(len(path)):
        for j in range(len(path[0])):
            newpath[i][j] = path[i][j]


#迭代那兩條表達式應用到除0和n外的每一個路徑點上
#迭代不會中止直到某次更新後整體變化比容忍偏差值小

change = tolerance
while (change >= tolerance):  
    change = 0
    for i in range(1,len(path)­1):
        for j in range(len(path[0])):
            d1 = weight_data * (path[i][j] - ­newpath[i][j])
            d2 = weight_smooth * (newpath[i­1][j] + newpath[i+1][j] - 2 * newpath[i][j])
            change += abs(d1 + d2)
            newpath[i][j] += d1 + d2
return newpath 

 

PID控制

假設車有可轉向的前輪和固定的後輪，以固定速率向前行駛，但願它走平滑的路徑，但只能控制前輪轉向角度。三種選擇：1.固定轉向量（這會走圓圈）2.隨機轉向命令，按橫切偏差的某個比例來設置轉向角度（橫切偏差CTE：車輛與參考航線間的垂直距離）3.讓轉向與橫切偏差成比例

應該選第三種，偏差越大就越朝着參考航線設置一個更大的轉向角，隨着接近航線，轉向角會變小，最終挨着航線。

這種轉向控制叫作P controller，應用這種控制到達航線後仍會以一個小轉角繼續行駛而後再，因此這種轉向控制下的行駛軌跡以下圖（臨界穩定）：

 

#使用以前robot類，編寫P控制器迭代100次機器人運動
#包含robot類中的__init__()、set()、set_noise()、move()、_repr_()
#轉向角 steer = -tau * crosstrack_error（CTE）import random
import numpy as np
import matplotlib.pyplot as plt

import proportional

def run(param):     #控制參數param就是轉向角度與CTE之比,換句話說param = tau = steer/CTE
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equalt to speed (we assume time = 1)
    N = 100 

    for i in range(N):
         crosstrack_error = myrobot.y
         steer = -param * crosstrack_error
         myrobot = myroobo.move(steer,speed)
         print myrobot, steer 

#run(0.1)

 

 下一個問題是有辦法避免越界嗎？用PD控制

在PD控制裏，轉向角度alpha不只由控制參數tau_p和橫切偏差CTE決定，也會與CTE關於時間的導數有關，

這裏間隔時間設爲1，那麼：

這個式子中，它會注意到CTE隨着時間的過去變小，當減少到必定程度它會反方向使車輪向上轉動，不會衝過 x軸而是平緩地接近參考航線。

因此如今不是像在P控制器裏以CTE的比例控制steer，還加入了第二個常數tau_d和CTE的微分項

def run(param1=0.2, param2=3.0):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 
    N = 100
    crosstrack_error = myrobot.y
    for i in range(N):
        diff_crosstrack_error = myrobot.y ­ crosstrack_error   #微分項
        crosstrack_error = myrobot.y
        steer = ­ param1 * crosstrack_errorparam2 * diff_crosstrack_error
        myrobot = myrobot.move(steer, speed)
        print myrobot, steer

#run(0.2,3.0)

 

 

這裏在初始時都是假定車輪對正前，但其實實際中車輪在開始時會有誤差角度，而這個問題P、PD都解決不了，（P會沿着一個常數震盪，PD會貼合一個常數），帶來系統誤差的問題，爲了解決這個問題，引入PID。

假如One people以系統偏差（由前輪轉向誤差帶來的持續的巨大誤差）的軌跡行駛，一段時間後他發現沒有接近預想軌跡，因而他開始往右打方向盤回到預想軌跡，這段時間的誤差能夠用CTE基於時間的積分來衡量。而後，來編寫新的控制器，它會根據CTE比例、CTE微分的某個比例、以及會受到CTE積分的比例影響，CTE積分就是歷史全部觀察到的CTE的和，它的累積能夠矯正方向。

def run(param1, param2, param3):
     myrobot = robot()
     myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0  #假定速度是1
    N = 100
    myrobot.set_steering_drift(10.0 / 180.0 * pi) # 初始轉向（形成系統誤差）
    int_crosstrack_error = 0 #新變量
    for i in range(N):
        int_crosstrack_error += crosstrack_error
        diff_crosstrack_error = myrobot.y ­ crosstrack_error
        crosstrack_error = myrobot.y
        steer = ­ param1 * crosstrack_error
                      -­ param2 * diff_crosstrack_error
    ­                  - param3 * int_crosstrack_error
        myrobot = myrobot.move(steer, speed)
        print myrobot, steer
    
#run(0.2,3.0,0.004)

 

參數優化

使用一種稱爲「Twiddle」的算法，每次只調整一個參數直到找到最佳參數集合。

從一個參數向量 p = [0, 0, 0]開始，p將表明迄今爲止最佳猜想參數集合，還將初始化一個向量dp = [1，1，1]，表明想嘗試的潛在變化。首先，在p中選擇一個參數（好比這個例子中的p [0]，儘管它能夠是任何參數），而且經過dp中的對應值增長該參數（例如，若是p [0] == 1和dp [0] == 0.5，那麼咱們p [0]爲1.5）。而後在run()中調用p，結果賦給best_error，而後遍歷p，首先把p增長一個探索值，把這個值賦給error，留下更小的值更新best_error，而後把dp的值稍微增大（自乘1.1），不然減掉兩個單位dp（由於以前加了一個單位），若是兩個都失敗了，p[i]變回原來的值，而且下降探索範圍dp[i]的值（bug應該是減dp[i]，好比自乘0.9）。只要dp之和大於閥值就會一直循環以上步驟。

 

#這裏的params是三維向量
def run(params, printflag = False):
     myrobot = robot()
     myrobot.set(0.0, 1.0, 0.0)
     speed = 1.0
     err = 0.0
     int_crosstrack_error = 0.0
     N = 100
     myrobot.set_steering_drift(10.0 / 180.0 * pi)  #設置了10度的起始轉向偏差
     crosstrack_error = myrobot.y

     for i in range(N * 2):  #迭代2*N次PID
         diff_crosstrack_error = myrobot.y ­ crosstrack_error
         crosstrack_error = myrobot.y
         int_crosstrack_error += crosstrack_error
         steer = ­ params[0] * crosstrack_error  \
            ­       - params[1] * diff_crosstrack_error \
            ­       - int_crosstrack_error * params[2]
         myrobot = myrobot.move(steer, speed)
         if i >= N:  #在N次之後纔開始統計，想觀察CTE更顯著的變化
              err += (crosstrack_error ** 2)
         if printflag:
              print myrobot, steer
     return err / float(N)  #返回平均偏差

def twiddle(tol = 0.2): 
     n_params = 3
     dparams = [1.0 for row in range(n_params)]
     params = [0.0 for row in range(n_params)]
     best_error = run(params)
     n = 0
     while sum(dparams) > tol:
           for i in range(len(params)):
               params[i] += dparams[i]
               err = run(params)
               if err < best_error:
                   best_error = err
                   dparams[i] *= 1.1
               else:
                   params[i] ­= 2.0 * dparams[i]
                   err = run(params)
                   if err < best_error:
                       best_error = err
                       dparams[i] *= 1.1
                   else:
                       params[i] += dparams[i]
                       dparams[i] *= 0.9
     n += 1
     print ‘Twiddle #’, n, params, ‘ ­> ‘, best_error
     return run(params)

 

實驗

下面嘗試把積分項去掉，獲得的是0積分增益，可是偏差比上面的那個偏差大一些，說明積分項對於讓偏差接近與0是必要的

而後嘗試一下，令dparams[1] = 0,把微分項去掉,出現了0.55的偏差！即便去掉drift偏差也是巨大的。

實驗在只有P狀況下偏差是0.1，PD下接近於0（沒有drift狀況下），而I（積分）是針對於有系統偏差的機器人。

 

尋路器、平滑器、控制器能夠應對大部分機器人的應用了，谷歌的還往裏面加了料，它考慮了轉向輪自有慣性的狀況，