> 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/odometry.md).

# Odometry

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

## 定義座標系統

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

<figure><img src="https://803424414-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FccbRdTJyjSEuuCcjRNJz%2Fuploads%2FfeYeoLD7tTGk1BrL1LNu%2Fcoordinate%20system.png?alt=media&amp;token=22010a42-4e8b-4782-9112-80df5f2b8611" alt="" width="375"><figcaption></figcaption></figure>

## 定義變數

$$\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 座標的變化定義為從起始點到機器最後位置畫出直線的長度，可以用此公式計算：

<figure><img src="https://803424414-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FccbRdTJyjSEuuCcjRNJz%2Fuploads%2FCKj3WsSNMSG6JtUgAlc1%2Flocal_delta_y.png?alt=media&amp;token=e4b17386-1a55-4675-a56c-b0abb445ebc3" alt="" width="563"><figcaption></figcaption></figure>

$$
\Delta y\_{local}=(\frac{\Delta R}{\Delta \theta} + s\_R) (2sin(\frac{1}{2} \Delta \theta))
$$

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

<figure><img src="https://803424414-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FccbRdTJyjSEuuCcjRNJz%2Fuploads%2Fm6d0I0o2jQDTULManWCR%2Flocal_delta_x.png?alt=media&amp;token=66779cc5-9c86-4efe-b23a-a2cfad2686f7" alt="" width="563"><figcaption></figcaption></figure>

$$
\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 變化為橫向輪角度變化。否則，以 [#ji-suan-local-zuo-biao-bian-hua](#ji-suan-local-zuo-biao-bian-hua "mention") 計算 local 座標變化
5. 把 local 座標變化轉換為 global 座標變化
6. 把 global 座標變化加起來，得到機器位置座標
7. 重複以上步驟

## 注意事項

1. 這個演算法必須在while loop裡跑，但是不能影響到機器主程序，所以也要把它放在一個獨立執行的Task裡。
2. 此算法一個迴圈能在一秒內跑越多次越好（盡量達到 50\~100次）