Pure Pursuit

Pure pursuit是一個路線追蹤演算法。此算法依賴預先計算好的曲線路徑( 路線生成)，以及Odometry ( Odometry )位置計算，讓機器走曲線。

演算法

  1. 以一個搜尋半徑，尋找在曲線路徑上的跟隨點

  2. 計算左右馬達的速度

尋找跟隨點

有很多種方法尋找跟隨點，其中有數學計算的方法，也有其他的方法。

方法1：計算跟隨點

詳情請見:

LogoBasic Pure PursuitBLRS Wiki
LogoCircle-Line Intersection -- from Wolfram MathWorldWolframResearch

我們for loop整個路線，在每次迴圈裡，找相鄰點 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)

Drawing

假設 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)為路線中相鄰的兩點，均相對於機器座標。設

dx=x2−x1d_x=x_2 - x_1
dy=y2−y1d_y=y_2-y_1
dr=dx2+dy2d_r=\sqrt{d_x^2+d_y^2}
D=∣x1x2y1y2∣=x1y2−y1x2D =\begin{vmatrix} x_1 & x_2 \\ y_1 & y_2 \end{vmatrix} = x_1y_2-y_1x_2

可以得到圓與線的交叉點：

x=Ddy±sgn(dy)dxr2dr2−D2dr2x = \frac{Dd_y \pm sgn(d_y)d_x \sqrt{r^2d_r^2 - D^2}}{d_r^2}

y=−Ddx±∣dy∣dxr2dr2−D2dr2y = \frac{-Dd_x \pm |d_y|d_x \sqrt{r^2d_r^2 - D^2}}{d_r^2}
sgn(x)={−1,if x<01,if x≥0}sgn(x) = \left\{\begin{array}{lr} -1, & \text{if } x<0\\ 1, & \text{if } x \geq 0 \end{array}\right\}

如果 r2dr2−D2<0\sqrt{r^2d_r^2 - D^2} < 0，則線和圓枚沒有交叉點

如果 r2dr2−D2=0\sqrt{r^2d_r^2 - D^2} = 0，則線和圓枚有一個交叉點

如果 r2dr2−D2>0\sqrt{r^2d_r^2 - D^2} > 0，則線和圓枚有兩個交叉點

檢查交叉點：

r2dr2−D2<0\sqrt{r^2d_r^2 - D^2} < 0，圓與線沒有交集
r2dr2−D2>0\sqrt{r^2d_r^2 - D^2} > 0，但是兩個交集點都沒有在範圍內，所以算是「沒有交集」
r2dr2−D2>0\sqrt{r^2d_r^2 - D^2} > 0，且兩個交集點都在範圍內，此時找靠近 (x2,y2)(x_2, y_2)的那個點

  1. 如果兩個交叉點都在 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)形成的範圍裡，設追蹤點為最靠近 (x2,y2)(x_2, y_2)的點

  2. 如果不是兩個交叉點都在 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)形成的範圍裡，設追蹤點為在範圍裡的交叉點

  3. 如果沒有交叉點都在 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)形成的範圍裡，則尋找距離機器最近的點

如果追蹤點距離路線的下一個點比機器距離下一個點更近，則停止迴圈，選擇其為真正的追蹤點。

方法2：尋找點出入圓的位置

此方法只能用於路線點夠密集，一定會有點在搜尋半徑內和搜尋半徑外時使用。此方法不同於方法1，可以使機器在狹窄的過彎中移動，或是繞過之前走過的路線，但是此方法的計算量較高，不適合太多點的路徑。但是在大多數VEX的情況，這個缺點沒有明顯的影響。

我們for loop整個路線，在每次迴圈裡，找相鄰點 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)

如果 (x1,y1)(x_1, y_1)在搜尋半徑內，而 (x2,y2)(x_2, y_2)在搜尋半徑外，且progress（設為距離機器最近的點）在 (x1,y1)(x_1, y_1)後面，代表兩點之間一定有要追尋的點，此時用二分逼進法尋找 (x1,y1),(x2,y2)(x_1, y_1), (x_2, y_2)線段和搜尋半徑的交集點（用少於10次的逼近即可）就可以找到跟隨點。progress也可以限制只能增加不能減少，或是有固定的增加速度。

如果路線上沒有點在搜尋半徑內，則設跟隨點為距離機器最近的點

追向跟隨點

有很多種方法計算左右馬達的速度，使機器追向跟隨點。其中有利用計算曲度的方法，也有結合PID控制演算法的方法，各有不同的特性。

方法1：計算曲度並控制底盤

https://www.researchgate.net/figure/Geometry-of-the-pure-pursuit-algorithm_fig1_328774686

我們可以透過此方法得到機器移動曲度：

x2+y2=p2x^2+y^2=p^2
x+q=rx+q=r
q=r−xq=r-x
(r−x)2+y2=r2(r-x)^2+y^2=r^2
r2−2rx+x2+y2=r2r^2-2rx+x^2+y^2=r^2
2rx=p22rx=p^2
r=p22xr=\frac{p^2}{2x}
κ=2xp2\kappa = \frac{2x}{p^2}

此時就可以用曲度 κ\kappa 計算左右輪的速度

vleft=vforward+vforward⋅κ⋅trackwidth2v_{left} = v_{forward} + v_{forward} \cdot \kappa \cdot \frac{trackwidth}{2}
vright=vforward−vforward⋅κ⋅trackwidth2v_{right} = v_{forward} - v_{forward} \cdot \kappa \cdot \frac{trackwidth}{2}

trackwidthtrackwidth為機器底盤的寬度

方法2：結合PID控制底盤

我們可以想像跟隨點和機器位置的差是一個距離誤差。為了修正誤差，就可以用PID演算法。此時正是如此。把橫向和縱向誤差分開，就可以用PID得到機器轉向輸出和移動輸出（注意：計算移動輸出時，不需要計算Integrated error，因為在機器到終點前，跟隨點和機器的距離永遠不變）。

我們可以設定 translational error 為 ylocalr\frac{y_{local}}{r}，r 為 搜尋半徑

我們也可以設定 rotational error 為 xlocalr\frac{x_{local}}{r}，r 為 搜尋半徑

此時就可以用PID處理這些誤差，轉換成左右輪的移動

比較

使用曲度的方法不會使機器在轉彎時減速，而是繞更大的彎來過急轉彎。在過彎曲度小的情況下可以更精準快速的移動。

Curvature based movement behavior

使用曲度的方法會使機器在轉彎時減速，而且允許讓機器倒著跑，這使機器做得到複雜的過彎動作，例如倒退轉彎。在過彎曲度大的情況下比較精準快速。如果比較熟悉PID，這種方法會更好進行動作調整優化。

PID based movement behavior

Visualize pure pursuit algorithm: https://acezxn.github.io/Pathtracker-online/#/path-follow-simulator

PreviousMotion ProfileNext路線生成

Last updated