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

1.Pure Pursuit原理

參考https://blog.csdn.net/weixin_39549161/article/details/88712443?spm=1001.2014.3001.5506

将小車簡化為兩輪自行車模型

個人了解純跟蹤算法就是讓無人駕駛車後軸中心，繞着轉彎中心，按轉彎半徑的圓弧走到待跟蹤路徑的預瞄點上，預瞄距離l越小，越快收斂到參考軌迹，車輛就越激進。

根據正弦定理

根據上圖的幾何關系，

聯立式(1),(2)

預瞄距離，如果為常數，在低速時效果可能很好，但在高速時太小的預瞄距離導緻車輛過于激進出現振蕩，是以通常讓預瞄距離正比于車速v.       防止車速很小時,l過小加上一個常數偏置。參數k，l0由自己設定，k越小越激進。

則(4)為最終的pure pursuit控制公式

2.python實作

算法原理非常簡單，由式4可以看出，關鍵隻要計算出預瞄距離，以及α即可求解目前時刻所需的轉向角。

2.1預瞄距離的計算

KLf= 0.1         #look forward gain
Lf0 = 3.0       #look-ahead distance，偏置
#calculate Lf
Lf=KLf*v+Lf0
           

2.2 α的計算

由下圖可以看出，

找到預瞄點後，直接用預瞄點和目前後輪中心計算連線與x軸所成角α_hat，然後α_hat減去航向角yaw即為α，代碼如下

#計算alpha
            alpha_hat = math.atan2(target_y - y,target_x - x)
            alpha = alpha_hat - yaw
           

2.3 預瞄點的确定

預瞄點要為我們設定計算公式計算出的預瞄距離，剛好相等可能比較困難，預瞄點有可能落在最近時刻的一小段參考軌迹waypoints裡的兩個參考點之間，周遊最近時刻的參考點，直到第i個參考點與小車後軸中心的距離剛大于預瞄距離時，則取第i個點為近似預瞄點,代碼如下：

#找Target
            for i in range(len(waypoints)):
                dx=self._current_x-waypoints[i][0]
                dy=self._current_y-waypoints[i][1]
                d = abs(math.sqrt(dx ** 2 + dy ** 2))
                if d>Lf:
                    target_x=waypoints[i][0]
                    target_y=waypoints[i][1]
                    break
           

3. Coursera Final Project說明

參見我另一篇部落格

https://blog.csdn.net/weixin_39199083/article/details/116723353?spm=1001.2014.3001.5502   2.1小節 

4. 運作結果

運作視訊： https://www.bilibili.com/video/BV18y4y1g7aA

完整代碼：https://download.csdn.net/download/weixin_39199083/18817439