> For the complete documentation index, see [llms.txt](https://acezxn.gitbook.io/vex-ji-qi-ren-cheng-shi-jiao-xue/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://acezxn.gitbook.io/vex-ji-qi-ren-cheng-shi-jiao-xue/jin-jie-jiao-xue/kong-zhi-yan-suan-fa/pure-pursuit.md). # Pure Pursuit Pure pursuit是一個路線追蹤演算法。此算法依賴預先計算好的曲線路徑( [路線生成](/vex-ji-qi-ren-cheng-shi-jiao-xue/jin-jie-jiao-xue/lu-xian-sheng-cheng.md))，以及Odometry ( [Odometry](/vex-ji-qi-ren-cheng-shi-jiao-xue/jin-jie-jiao-xue/kong-zhi-yan-suan-fa/odometry.md) )位置計算，讓機器走曲線。

## 演算法 1. 以一個搜尋半徑，尋找在曲線路徑上的跟隨點 2. 計算左右馬達的速度 ## 尋找跟隨點有很多種方法尋找跟隨點，其中有數學計算的方法，也有其他的方法。 ### 方法1：計算跟隨點詳情請見: {% embed url="" %} {% embed url="" %} 我們for loop整個路線，在每次迴圈裡，找相鄰點 $$(x\_1, y\_1), (x\_2, y\_2)$$

假設 $$(x\_1, y\_1), (x\_2, y\_2)$$為路線中相鄰的兩點，均相對於機器座標。設 $$ d\_x=x\_2 - x\_1 $$ $$ d\_y=y\_2-y\_1 $$ $$ d\_r=\sqrt{d\_x^2+d\_y^2} $$ $$ D =\begin{vmatrix} x\_1 & x\_2 \ y\_1 & y\_2 \end{vmatrix} = x\_1y\_2-y\_1x\_2 $$ 可以得到圓與線的交叉點： $$ x = \frac{Dd\_y \pm sgn(d\_y)d\_x \sqrt{r^2d\_r^2 - D^2}}{d\_r^2} $$ $$ y = \frac{-Dd\_x \pm |d\_y|d\_x \sqrt{r^2d\_r^2 - D^2}}{d\_r^2} $$ $$ sgn(x) = \left{\begin{array}{lr} -1, & \text{if } x<0\\ 1, & \text{if } x \geq 0 \end{array}\right} $$ 如果 $$\sqrt{r^2d\_r^2 - D^2} < 0$$，則線和圓枚沒有交叉點如果 $$\sqrt{r^2d\_r^2 - D^2} = 0$$，則線和圓枚有一個交叉點如果 $$\sqrt{r^2d\_r^2 - D^2} > 0$$，則線和圓枚有兩個交叉點檢查交叉點：

\sqrt{r^2d_r^2 - D^2} > 0，但是兩個交集點都沒有在範圍內，所以算是「沒有交集」

\sqrt{r^2d_r^2 - D^2} > 0，且兩個交集點都在範圍內，此時找靠近 (x_2, y_2)的那個點

1. 如果兩個交叉點都在 $$(x\_1, y\_1), (x\_2, y\_2)$$形成的範圍裡，設追蹤點為最靠近 $$(x\_2, y\_2)$$的點 2. 如果不是兩個交叉點都在 $$(x\_1, y\_1), (x\_2, y\_2)$$形成的範圍裡，設追蹤點為在範圍裡的交叉點 3. 如果沒有交叉點都在 $$(x\_1, y\_1), (x\_2, y\_2)$$形成的範圍裡，則尋找距離機器最近的點如果追蹤點距離路線的下一個點比機器距離下一個點更近，則停止迴圈，選擇其為真正的追蹤點。 ### 方法2：尋找點出入圓的位置此方法只能用於路線點夠密集，一定會有點在搜尋半徑內和搜尋半徑外時使用。此方法不同於方法1，可以使機器在狹窄的過彎中移動，或是繞過之前走過的路線，但是此方法的計算量較高，不適合太多點的路徑。但是在大多數VEX的情況，這個缺點沒有明顯的影響。我們for loop整個路線，在每次迴圈裡，找相鄰點 $$(x\_1, y\_1), (x\_2, y\_2)$$

如果 $$(x\_1, y\_1)$$在搜尋半徑內，而 $$(x\_2, y\_2)$$在搜尋半徑外，且progress（設為距離機器最近的點）在 $$(x\_1, y\_1)$$後面，代表兩點之間一定有要追尋的點，此時用二分逼進法尋找 $$(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

我們可以透過此方法得到機器移動曲度： $$ x^2+y^2=p^2 $$ $$ x+q=r $$ $$ q=r-x $$ $$ (r-x)^2+y^2=r^2 $$ $$ r^2-2rx+x^2+y^2=r^2 $$ $$ 2rx=p^2 $$ $$ r=\frac{p^2}{2x} $$ $$ \kappa = \frac{2x}{p^2} $$ 此時就可以用曲度 $$\kappa$$ 計算左右輪的速度 $$ v\_{left} = v\_{forward} + v\_{forward} \cdot \kappa \cdot \frac{trackwidth}{2} $$ $$ v\_{right} = v\_{forward} - v\_{forward} \cdot \kappa \cdot \frac{trackwidth}{2} $$ $$trackwidth$$為機器底盤的寬度 ### 方法2：結合PID控制底盤我們可以想像跟隨點和機器位置的差是一個距離誤差。為了修正誤差，就可以用PID演算法。此時正是如此。把橫向和縱向誤差分開，就可以用PID得到機器轉向輸出和移動輸出（注意：計算移動輸出時，不需要計算Integrated error，因為在機器到終點前，跟隨點和機器的距離永遠不變）。我們可以設定 translational error 為 $$\frac{y\_{local}}{r}$$，r 為搜尋半徑我們也可以設定 rotational error 為 $$\frac{x\_{local}}{r}$$，r 為搜尋半徑此時就可以用PID處理這些誤差，轉換成左右輪的移動 ### 比較使用曲度的方法不會使機器在轉彎時減速，而是繞更大的彎來過急轉彎。在過彎曲度小的情況下可以更精準快速的移動。

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

Visualize pure pursuit algorithm: