Pure Pursuit算法在移動機器人路徑跟蹤的位姿修正研究

王 勇 （安徽合力股份有限公司，安徽 合肥 230022）

1 概述

Pure Pursuit算法原理簡明易懂，不需要高階數(shù)學(xué)計算和繁瑣過程推導(dǎo)，可以用于顯式或隱式路徑跟蹤問題，特別適合移動機器人路徑跟蹤控制。當對移動機器人行駛方向與到達目標點無嚴格方向要求時，可以直接采用Pure Pursuit算法，控制移動機器人沿路徑行駛或跟隨某導(dǎo)向物體行駛。但在大多數(shù)情況下，移動機器人行駛到達路徑上的目標點時，一般對機器人的朝向都有嚴格要求。對于采用Pure Pursuit算法的機器人必須在到目標點之前對機器人行駛方向進行調(diào)整，以滿足機器人到達目標位置時的方向要求。如裝卸貨物的AGV叉車，不僅要求車輛準確到達目標點位置，而且還要求貨叉與被裝卸貨物托盤的叉孔保持平行。此時，對于采用Pure Pursuit算法的移動機器人就必須對Pure Pursuit算法進行修正，以滿足機器人在目標點的方向要求。

2 Pure Pursuit算法原理

Pure Pursuit算法原理如圖1所示。為分析方便，假定移動機器人局部坐標系原點位于左、右兩驅(qū)動輪軸線與車輛中心線交點SP上，X、Y軸按右手規(guī)則確定，Y軸正向為移動機器人前進方向。假定目標點GP位置已知，此時通過控制左、右驅(qū)動輪不同速度，使車輛始終沿Y軸切線方向按曲率γ的圓弧曲線行進，在出發(fā)點到目標點之間不斷校正當前與目標位置的誤差，車輛即可準確到達目標點。算法中規(guī)定曲率γ是帶符號的參數(shù)，當車輛順時針沿曲率γ行駛時γ取負值，反之取正值。

移動機器人從出發(fā)點SP到目標點GP曲率γ由下式計算得到：

式中：L——移動機器人從出發(fā)點SP到目標點GP距離；

圖1 Pure Pursuit算法原理

xg——目標點GP在局部坐標系XY坐標系中的X坐標值。

3 算法修正

由Pure Pursuit算法可知，當移動機器從SP出發(fā)到GP時雖然能夠滿足位置要求，但是不能保證機器人與目標點的方向要求。如圖2所示，移動機器從SP到達GP后方與目標點方向差異。

為了保證移動機器到達GP點后方向仍然與目標點GP要求方向相同，此時需要對Pure Pursuit算法進行適當修正。修正方法是：仍然保持Pure Pursuit算法的基本思想，在SP和GP之間增加一個過渡點TP。SP到TP移動機器人沿曲率γ1行駛，到達TP點后，按曲率γ2行駛。從SP到TP的路徑曲線γ1與TP到GP的路徑曲線γ2相切，γ2與目標點GP所要求的方向相同。這樣修正后的Pure Pursuit算法不僅保持從SP到GP的位置，同時還能保持移動機器人到達GP后的方向（Y軸正向）與GP點要求的指向相同。對Pure Pursuit算法的修正見圖3，在SP與GP之間增加中間點TP。

圖2 機器人方向與目標點方向差異

事實上，在SP到GP之間滿足上述修正條件要求的TP點有無窮多個，如何確定一個實用的TP點就成為修正后的Pure Pursuit算法的關(guān)鍵。按照圖3，下面給出了一種簡便切實可行求取TP點的參考方法。

圖3 增加中間點TP調(diào)整移動機器人方向

首先，假定GP點在局部坐標系第一象限（第二象限時有類似方法，不再贅述），按控制律求出移動機器人從SP沿曲率為γ的路徑曲線到達GP時切線方向β，判斷目標點GP的方向α與β之間的關(guān)系：

（1） β+π≥α>β時，在方向軸線上方插入 TP1點，并令 γ2＞0。

（2） α=β時，不需要中間點TP點。

（3） β>α≥β-π 時，在方向軸線下方插入TP2點，并令 γ2＜0。

其次，為移動機器人從TP1（或TP2）點沿曲率γ2到GP點的半徑值，這里使用SP到GP的距離為L，令：

在幾何上當曲率γ2過小，從SP到達GP點既使得曲率γ1與曲率γ2兩路徑曲線在TP1（或TP2） 相切，同時還滿足曲率γ2在GP點與目標方向相同的中間點可能并不存在。因此，依據(jù)SP到GP之間的距離來確定恰當?shù)那师?是非常必要的。一般取0.1L≥1/γ2≥0.25L。這里是通過事先確定曲率γ2來求取曲率γ1，如果先確定曲率γ1再求曲率γ2，求解過程的條件判斷繁瑣，不推薦不再贅述。

最后，確定曲率γ2之后（含正負），從SP到GP的中間點TP1（或TP2）就唯一確定，利用SP為原點的局部坐標系中幾何關(guān)系，可推導(dǎo)得出以下解析式：

式中：α——目標點GP與x軸正向所成角度（逆時針為正，順時針為負）；

?1——從SP點到TP1（或TP2） 轉(zhuǎn)角（逆時針為正，順時針為負）；

a,b——目標點GP的x,y坐標值；

TPx,TPy——TP1（或TP2） 的x,y坐標值。

4 程序驗證

下面通過程序分別對比了未進行修正前的Pure Pursuit算法和采用中間點進行修正后的Pure Pursuit算法程序仿真驗證情況。圖4為未進行修正的Pure Pursuit算法的路徑跟蹤情況，圖5為修正后的Pure Pursuit算法路徑跟蹤情況。從圖4中可以看到，算法在未修正前雖然能很好地跟蹤目標路徑上的關(guān)鍵點，但關(guān)鍵點處的方向與所要求的方向并不吻合；從圖5中看到采用修正后的算法，機器人不僅與目標路徑曲線跟隨吻合，方向與路徑方向完全相同。

圖4 未修正Pure Pursuit情況

圖5 修正后Pure Pursuit情況

5 結(jié)論

論文對Pure Pursuit算法的原理和局限性進行了闡述，給出了算法修正的方法，并對修正后的算法進行了計算仿真，驗證了修正方法的正確性。論文對采用Pure Pursuit算法進行路徑跟蹤的移動機器人在行駛方向有要求時提供了算法參考。