Pure Pursuit

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

演算法

以一個搜尋半徑，尋找在曲線路徑上的跟隨點
計算左右馬達的速度

尋找跟隨點

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

方法1：計算跟隨點

詳情請見:

我們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$ ，則線和圓枚有兩個交叉點

檢查交叉點：

如果兩個交叉點都在 $(x_1, y_1), (x_2, y_2)$ 形成的範圍裡，設追蹤點為最靠近 $(x_2, y_2)$ 的點
如果不是兩個交叉點都在 $(x_1, y_1), (x_2, y_2)$ 形成的範圍裡，設追蹤點為在範圍裡的交叉點
如果沒有交叉點都在 $(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也可以限制只能增加不能減少，或是有固定的增加速度。