Odometry

Odometry是一個位置偵測的系統，可以用來計算機器在場地上的座標位置

定義座標系統

在計算位置時，必須先有一套座標系統。在此設定做表的原點在場地俯視圖的左上角，而 x 隨著機器往右提升，y隨著機器往下提升。

定義變數

$\Delta L$：左邊輪子的角度變化

$\Delta R$：右邊輪子的角度變化

$\Delta S$：橫向輪子的角度變化（假設此輪在車後）

$\theta$：機器的面相角度

$\Delta \theta$：機器的面相角度變化

$s_L$：左邊輪子距離中心的距離

$s_R$：右邊輪子距離中心的距離

$s_S$：後面橫向輪子距離中心的距離

$\Delta x_{local}$：local x座標變化

$\Delta y_{local}$：local y座標變化

$\Delta x_{global}$：global x座標變化

$\Delta y_{global}$：global y座標變化

計算機器面相角度變化

角度的變化量會直接影響到後續計算的座標變化。如果機器有裝角度感應器，如Inertial Sensor，就可以用感應器讀到的值算角度變化。如果沒有，就需要靠左右輪計算。設 $r$ 為機器的旋轉半徑，可以用扇形公式得到左右輪的角度變化：

$\Delta L = (r + s_L) \Delta \theta$ $\Delta R = (r-s_R) \Delta \theta$

再把兩個算式簡化，得到：

$\frac {\Delta L}{\Delta \theta} = r+s_L$ $\frac {\Delta R}{\Delta \theta} = r-s_R$

$r=\frac {\Delta L}{\Delta \theta}-s_L = \frac {\Delta R}{\Delta \theta}+s_R$

忽略 $r$ ，再進行簡化，得到：

$\Delta L -s_L\Delta \theta = \Delta R+s_R\Delta \theta$

$\Delta L - \Delta R = (s_L+s_R)\Delta \theta$

最後得到機器面向角度變化：

$\Delta \theta = \frac {\Delta L - \Delta R}{s_L + s_R}$

計算local座標變化

我們可以把 local y 座標的變化定義為從起始點到機器最後位置畫出直線的長度，可以用此公式計算：

$$\Delta y_{local}=(\frac{\Delta R}{\Delta \theta} + s_R) (2sin(\frac{1}{2} \Delta \theta))$$

同樣的，也可以用此公式計算local x的變化:

$$\Delta x_{local}=(\frac{\Delta S}{\Delta \theta} + s_S) (2sin(\frac{1}{2}\Delta \theta))$$

轉換到Global座標

最後用旋轉矩陣轉換到Global:

$$\Delta x_{global}=\Delta y_{local} \cdot cos(\theta - \frac{1}{2}\Delta \theta) - \Delta x_{local} \cdot sin(\theta - \frac{1}{2}\Delta \theta)$$

$$\Delta y_{global}=\Delta y_{local} \cdot sin(\theta - \frac{1}{2}\Delta \theta) + \Delta x_{local} \cdot cos(\theta - \frac{1}{2}\Delta \theta)$$

演算法

1. 測量左輪、右輪、橫向輪角度

2. 計算左輪、右輪、橫向輪角度變化

3. 計算機器面向角度變化，和機器的面向角度（如果有Inertial Sensor，可以直接讀值，並計算角度變化）

4. 如果角度變化為零，則 local y 變化為左輪、右輪的角度變化；local x 變化為橫向輪角度變化。否則，以 計算local座標變化 計算 local 座標變化

5. 把 local 座標變化轉換為 global 座標變化

6. 把 global 座標變化加起來，得到機器位置座標

7. 重複以上步驟

注意事項

1. 這個演算法必須在while loop裡跑，但是不能影響到機器主程序，所以也要把它放在一個獨立執行的Task裡。

2. 此算法一個迴圈能在一秒內跑越多次越好（盡量達到 50~100次）