Coursera self-driving car Part1 Final Project——自動駕駛軌迹跟蹤之Stanley算法推導及Python實作

同樣，該Stanley算法也是用于Coursera self driving car Part1 FinalProject裡得橫向控制，縱向采用PID控制不贅述。

1.Stanley算法原理

Stanley算法原理相對比較簡單，可以處理初始橫向誤差，航向誤差較大得情況。

Stanley控制量前輪轉角得産生由兩部分構成：

為了消除車輛航向與參考航向之間得角度差所需得前輪轉角，參考航向即為參考點得切線方向

為了消除橫向距離需要得前輪轉角

 為 我們給定的距離，但如果設定為一個常值，越小，車子就越快接近參考路徑，轉向角就越大。低速時可能适用，高速時就使車輛變得非常激進，使得系統出現振蕩，是以我們通常将其設定為一個與v正比的值

這個比例系數k越大，

 就越小，車輛就越激進。k作為Stanley控制器的一個參數，類似與PID的比例控制，還可以進行改造加入微分積分環節，這裡不再贅述。

原理就是這樣，很簡單。

2.Coursera self driving car Part1 FinalProject介紹

Part1 FinalProject參見我得另一篇部落格裡的2.1小節

Coursera self-driving car Part1 Final Project——無人車軌迹跟蹤之MPC模型預測控制原理推導及Python實作

3.Python實作

這裡隻介紹關鍵代碼的實作.

這個Final project裡的設定是提供的waypoints是一個點集，每個時刻都會更新，詳見本部落格第2小節。waypoints了解為接下來racetrack_waypoints.txt裡的幾個參考點進行線性插值後的點集。

算法代碼實作大緻流程：

參數初始化→找到目前最近點與最近距離(橫向誤差)→計算參考航向→判斷橫向誤差符号→計算

→下發控制量

3.1 找到附近小段軌迹中的距離車輛的最近點

思路就是周遊waypoints小段軌迹點集，依次求出距離，初始最近距離設定為無窮大，最近點索引為0，然後後面若發現有更小的距離，則更新最近距離min_dist和最近點索引 min_idx。

#找到最近的路徑點索引
            min_idx       = 0
            min_dist      = float("inf")
            for i in range(len(self._waypoints)):          
               dist = np.linalg.norm(np.array([
                    self._waypoints[i][0] - x_f,
                    self._waypoints[i][1] - y_f]))   #出現“Unexpected indent”原因可能之一，可能是你的函數或哪一行的縮進出了問題，if 跟dist縮進一樣的屬于for循環下
               if dist < min_dist:               #每個i比較是否比min_dist要小，将if放錯位置縮進不對頂格導緻每次都取了最後一個i，因為無論距離多少肯定比inf小導緻控制量很大原地轉圈
                        min_dist = dist
                        min_idx = i 
           

3.2 參考航向的計算

Final project的設定是車輛的實際狀态x,y,phi,v都會由傳感器給出數值，實際航向角（車速方向與x軸所成角，-pi-pi）為yaw，但是如上原理所述Stanley算法中要用到參考航向角。

參考航向角的計算思路：

程式中跟蹤路徑橫向控制時是将路徑距車輛最近點作為參考點，這裡将最近點與其下一個點的連線方向作為參考航向(若最近點是點集中的最後一個點，則采用最近點上一個點與最近點的連線作為參考航向)，代碼如下：

atan2計算出來的角度會自動轉換到-pi到pi，航向角的設定就是-pi到pi

#設定目标點
            tx=waypoints[min_idx][0]
            ty=waypoints[min_idx][1]
            #判斷用下一個點還是上一個點與目前目标點的連線作為參考航向
            ##重要！！！因為yaw 這裡定義的範圍是有負值的而不是0-2pi，是-pi到pi，那麼th也-pi到Pi即可，否則有問題
            if min_idx<len(self._waypoints)-1:
                #不是最後一個點就用下一個點計算參考航向
                th=math.atan2(waypoints[min_idx+1][1]-waypoints[min_idx][1],waypoints[min_idx+1][0]-waypoints[min_idx][0])
            else:
                th=math.atan2(waypoints[min_idx][1]-waypoints[min_idx-1][1],waypoints[min_idx][0]-waypoints[min_idx-1][0])
           

3.3 判斷橫向誤差的符号

看了幾個CSDN上的排在前面的答案，移植到python中做這個FinalProject時，發現都是有漏洞的，用來做這個project經常沖出跑道。看的幾個部落格常見的一種思路，就是：

簡單的把車在參考點切線的右側(Axr+Byr+C>0)作為負(Coursera Finalproject裡定義向左轉為負的轉向角)，車子在在參考點切線的左側(Axr+Byr+C<0)作為正，簡單的這樣判斷方向，當同一段軌迹，小車是向右上方運動時，Ax+By+C>0左轉還沒錯，

當小車是反向向左下方跟蹤軌迹時，Ax+By+C>0左轉就有問題了，反而偏離了軌迹。

下面介紹一種在Github上看到的新方法。

用參考路徑的第一個參考點P0與小車位置P的連線與x軸所成角

,最近參考點的切線方向的航向角

，這種角度均在-pi到pi之間，航向角是-pi-pi，atan2計算出來的角度也是-pi到pi。

上圖如果小車向右上方運動

，左拐沒問題，橫向誤差為負。

如果小車向左下方運動，

小車需向右拐，則橫向誤差為正，也正确。

在Finalproject裡坐标系是反的，則上述規則剛好反過來即可。

代碼如下：

#判斷error的符号
            #計算第一個參考點到目前位置的連線與x軸所成角
            yaw_cross_track = np.arctan2(y_f-waypoints[0][1], x_f-waypoints[0][0])
            #waypoints裡的第一個點到目前位置連線角度與參考航向之間的夾角計算
            yaw_path2ct = th - yaw_cross_track
            if yaw_path2ct > np.pi:
                yaw_path2ct -= 2 * np.pi
            if yaw_path2ct < -np.pi:
                yaw_path2ct += 2 * np.pi
            # yaw_path2ct>0就是正的
            if yaw_path2ct > 0:
               error = abs(error)
            else:
               error = - abs(error)
            #
           

3.4 角度加減後範圍的限制

整個程式裡的角度範圍都是-pi到pi，無論是航向角還是求反正切出來的值，

但是兩個角度加減後可能超出範圍，都要轉化到這個範圍内。代碼如下：

#waypoints裡的第一個點到目前位置連線角度與參考航向之間的夾角計算
            yaw_path2ct = th - yaw_cross_track
            if yaw_path2ct > np.pi:
                yaw_path2ct -= 2 * np.pi
            if yaw_path2ct < -np.pi:
                yaw_path2ct += 2 * np.pi
           

3.5 運作結果

與我另外兩篇部落格介紹的Pure Pursuit純跟蹤, MPC橫向控制并沒有看出明顯優劣，因為參數的設定相對都比較随意。但是都能夠很好的完成軌迹跟蹤的任務。

運作視訊連結

https://www.bilibili.com/video/BV1kB4y1F7sU

源碼連結

https://download.csdn.net/download/weixin_39199083/18814608