TSXCAY21 Premium 伺服運動模塊

通過點到直線的距離運算可以得到d14，d15和d16的實際長度，由于在移動過程中使用的是同一個mark點，d1，d2，d3在相機1和相機2的視野中的值是一樣的。從而可以得到Point10在機械手坐標系中的實際坐標為： Point10X=X1［1］+d16+d2，Point10Y=Y1［1］+（d1-d15）。

同樣可以得到Point20在機械手坐標系中的實際坐標：

Point20X=X2［1］+d26+d2，Point20Y=Y2［1］+（d1-d25）。

求取該標準線斜率;

K = （Point20Y-Point10Y） / （Ponit20X-Point20X）

= （Y2［1］-Y1［1］-d25+d15） / （X2［1］-X1［1］+d26-d16）

之后在每次定位時需要和此標準線的斜率進行比較，從而得到和標準線的夾角，最后進行旋轉校正。

注意：由于Point10和Point20會落在不同的位置，在計算Point10和Point20的實際坐標時上述公式會有所變化，但原理相同。

定位校準

以后在每次進行檢測時，得到的結果都和標準線進行比較，可以得到和標準線的夾角和相對于標準位置的位置偏差，根據得到的角度和位置偏差進行校正。在校正過程中需要先旋轉后平移，做一個閉環反饋系統實時校正。